├── README.md ├── arduino_code └── arduino_code.ino ├── circuit.png ├── moveit_pkg ├── CMakeLists.txt ├── config │ ├── arduino_robot_arm.srdf │ ├── chomp_planning.yaml │ ├── fake_controllers.yaml │ ├── joint_limits.yaml │ ├── kinematics.yaml │ ├── ompl_planning.yaml │ ├── ros_controllers.yaml │ └── sensors_3d.yaml ├── launch │ ├── arduino_robot_arm_moveit_controller_manager.launch.xml │ ├── arduino_robot_arm_moveit_sensor_manager.launch.xml │ ├── chomp_planning_pipeline.launch.xml │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── demo_gazebo.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── gazebo.launch │ ├── joystick_control.launch │ ├── move_group.launch │ ├── moveit.rviz │ ├── moveit_rviz.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── ros_controllers.launch │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── trajectory_execution.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml ├── package.xml ├── scripts │ ├── color_thresholding.py │ └── get_pose_openCV.py └── src │ └── move_group_interface.cpp ├── positions.png └── robot_arm_pkg ├── CMakeLists.txt ├── config └── ros_controllers.yaml ├── launch ├── check_motors.launch ├── check_motors_gazebo.launch ├── config.rviz ├── gazebo_urdf.launch ├── rviz_urdf.launch └── urdf_config.rviz ├── meshes └── stl │ ├── Arm 01.stl │ ├── Arm 02.stl │ ├── Base.stl │ ├── Cube.002.stl │ ├── Gripper.001.stl │ ├── Gripper.stl │ ├── Waist.001.stl │ └── Waist.stl ├── package.xml ├── scripts └── joint_states_to_gazebo.py └── urdf └── arduino_robot_arm.urdf /README.md: -------------------------------------------------------------------------------- 1 | # arduino_robot_arm 2 | ROS packages that can be used to plan and execute motion trajectories for a robot arm in simulation and real-life. 3 | 4 | 5 | 6 | These packages were tested under ROS kinetic and Ubuntu 16.04 and it works perfectly on ROS melodic and noetic 7 | 8 | The robot arm uses Moveit plugin to apply kinematics by the KDL solver. These packages can be tested in the gazebo simulation tool and the real robot arm, where the ROS system and Arduino code share the ```/joint_states``` topic to control motors. 9 | 10 | 11 | ## Dependencies 12 | run this instruction inside your workspace: 13 | 14 | ```$ rosdep install --from-paths src --ignore-src -r -y``` 15 | 16 | make sure you installed all these packages: 17 | 18 | for kinetic distro 19 | 20 | ``` 21 | $ sudo apt-get install ros-kinetic-moveit 22 | $ sudo apt-get install ros-kinetic-joint-state-publisher ros-kinetic-joint-state-publisher-gui 23 | $ sudo apt-get install ros-kinetic-gazebo-ros-control joint-state-publisher 24 | $ sudo apt-get install ros-kinetic-ros-controllers ros-kinetic-ros-control 25 | ``` 26 | 27 | for melodic distro 28 | 29 | ``` 30 | $ sudo apt-get install ros-melodic-moveit 31 | $ sudo apt-get install ros-melodic-joint-state-publisher ros-melodic-joint-state-publisher-gui 32 | $ sudo apt-get install ros-melodic-gazebo-ros-control joint-state-publisher 33 | $ sudo apt-get install ros-melodic-ros-controllers ros-melodic-ros-control 34 | ``` 35 | 36 | for noetic distro 37 | 38 | ``` 39 | $ sudo apt-get install ros-noetic-moveit 40 | $ sudo apt-get install ros-noetic-joint-state-publisher ros-noetic-joint-state-publisher-gui 41 | $ sudo apt-get install ros-noetic-gazebo-ros-control joint-state-publisher 42 | $ sudo apt-get install ros-noetic-ros-controllers ros-noetic-ros-control 43 | ``` 44 | 45 | ## Robot Arm 46 | The robot arm has 5 joints only 4 joints can be fully controlled via ROS and Rviz, the last joint (gripper) has a default motion executed from the Arduino code directly. 47 | ### Circuit diagram 48 | ![circuit](circuit.png) 49 | ### Robot initial positions 50 | ![positions](positions.png) 51 | 52 | ## Configuring Arduino with ROS 53 | - Install Arduino IDE in Ubuntu 54 | https://www.arduino.cc/en/software 55 | to install run ```$ sudo ./install.sh``` after unzipping the folder 56 | 57 | - Launch the Arduino IDE 58 | 59 | - Install the arduino package and ros library 60 | http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup 61 | 62 | - Make sure to change the port permission before uploading the Arduino code 63 | ```$ sudo chmod 777 /dev/ttyUSB0``` 64 | 65 | ## Usage 66 | ### Controlling the robot arm by joint_state_publisher 67 | ```$ roslaunch robot_arm_pkg check_motors.launch``` 68 | 69 | You can also connect with hardware by running: 70 | 71 | ```$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200``` 72 | 73 | (Note: You may need to use ttyACM) 74 | 75 | #### Simulation 76 | Run the following instructions to use gazebo 77 | ``` 78 | $ roslaunch robot_arm_pkg check_motors.launch 79 | $ roslaunch robot_arm_pkg check_motors_gazebo.launch 80 | $ rosrun robot_arm_pkg joint_states_to_gazebo.py 81 | ``` 82 | (You may need to change the permission) 83 | 84 | ```$ sudo chmod +x ~/catkin_ws/src/arduino_robot_arm/robot_arm_pkg/scripts/joint_states_to_gazebo.py``` 85 | 86 | 87 | ### Controlling the robot arm by Moveit and kinematics 88 | ```$ roslaunch moveit_pkg demo.launch``` 89 | 90 | You can also connect with hardware by running: 91 | 92 | ```$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200``` 93 | 94 | (Note: You may need to use ttyACM) 95 | 96 | #### Simulation 97 | Run the following instruction to use gazebo 98 | 99 | ```$ roslaunch moveit_pkg demo_gazebo.launch``` 100 | 101 | *** 102 | 103 | # Pick and place by using OpenCV 104 | ## Preparation 105 | Download webcam extension for VirtualBox 106 | 107 | https://scribles.net/using-webcam-in-virtualbox-guest-os-on-windows-host/ 108 | 109 | ## Testing the camera and OpenCV 110 | 111 | Run color_thresholding.py to test the camera 112 | 113 | Before running, find the camera index normally it is video0 114 | 115 | ```$ ls -l /dev | grep video``` 116 | 117 | If it is not, update line 8 in color_thresholding.py 118 | 119 | ```8 cap=cv2.VideoCapture(0)``` 120 | 121 | Then run 122 | 123 | ```$ python color_thresholding.py``` 124 | 125 | ## Using OpenCV with the robot arm in ROS 126 | ### In Real Robot 127 | - In a terminal run 128 | 129 | ```$ roslaunch moveit_pkg demo.launch``` 130 | 131 | this will run Rviz 132 | 133 | 134 | - connect with Arduino: 135 | 136 | 1. select the Arduino port to be used on Ubuntu system 137 | 138 | 2. change the permissions (it might be ttyACM) 139 | 140 | ```$ ls -l /dev | grep ttyUSB``` 141 | 142 | ```$ sudo chmod -R 777 /dev/ttyUSB0``` 143 | 144 | 3. upload the code from Arduino IDE 145 | 146 | ```$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200``` 147 | 148 | - In another terminal 149 | 150 | ```$ rosrun moveit_pkg get_pose_openCV.py``` 151 | 152 | This will detect **blue** color and publish the x,y coordinates to /direction topic 153 | 154 | (Note: check the camera index and update the script if needed) 155 | 156 | - Open another terminal 157 | 158 | ```$ rosrun moveit_pkg move_group_node``` 159 | 160 | This will subscribe to /direction topic and execute motion by using **Moveit move group** 161 | 162 | The pick and place actions are performed from the Arduino sketch directly. 163 | 164 | 165 | ### In simulation (Gazebo) 166 | - In a terminal run 167 | 168 | ```$ roslaunch moveit_pkg demo_gazebo.launch``` 169 | 170 | this will run Rviz and gazebo 171 | 172 | - In another terminal 173 | 174 | ```$ rosrun moveit_pkg get_pose_openCV.py``` 175 | 176 | This will detect **blue** color and publish the x,y coordinates to /direction topic 177 | 178 | (Note: check the camera index and update the script if needed) 179 | 180 | - Open another terminal 181 | 182 | ```$ rosrun moveit_pkg move_group_node``` 183 | 184 | This will subscribe to /direction topic and execute motion by using Moveit move group 185 | 186 | **We can’t visualize the pick and place actions in gazebo** 187 | -------------------------------------------------------------------------------- /arduino_code/arduino_code.ino: -------------------------------------------------------------------------------- 1 | #if defined(ARDUINO) && ARDUINO >= 100 2 | #include "Arduino.h" 3 | #else 4 | #include 5 | #endif 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | ros::NodeHandle nh; 13 | Servo gripper; 14 | Servo wrist; 15 | Servo elbow; 16 | Servo shoulder; 17 | Servo base; 18 | 19 | double base_angle=90; 20 | double shoulder_angle=90; 21 | double elbow_angle=90; 22 | double wrist_angle=90; 23 | 24 | double prev_base = 0; 25 | double prev_shoulder = 0; 26 | double prev_elbow = 0; 27 | double prev_wrist = 0; 28 | 29 | int gripperState = 0; 30 | int positionChanged = 0; 31 | 32 | void servo_cb(const sensor_msgs::JointState& cmd_msg){ 33 | base_angle=radiansToDegrees(cmd_msg.position[0]); 34 | shoulder_angle=radiansToDegrees(cmd_msg.position[1]); 35 | elbow_angle=radiansToDegrees(cmd_msg.position[2]); 36 | wrist_angle=radiansToDegrees(cmd_msg.position[3]); 37 | 38 | base.write(base_angle); 39 | shoulder.write(shoulder_angle); 40 | elbow.write(elbow_angle); 41 | wrist.write(wrist_angle); 42 | 43 | if (prev_base==base_angle && prev_shoulder==shoulder_angle && prev_elbow==elbow_angle && prev_wrist==wrist_angle && positionChanged==0) 44 | { 45 | if (gripperState==0) 46 | { 47 | gripper.write(60); 48 | gripperState = 1; 49 | } 50 | else if (gripperState==1) 51 | { 52 | gripper.write(0); 53 | gripperState = 0; 54 | } 55 | positionChanged = 1; 56 | } 57 | else if ((prev_base!=base_angle || prev_shoulder!=shoulder_angle || prev_elbow!=elbow_angle || prev_wrist!=wrist_angle) && positionChanged==1) 58 | { 59 | positionChanged = 0; 60 | } 61 | 62 | prev_base = base_angle; 63 | prev_shoulder = shoulder_angle; 64 | prev_elbow = elbow_angle; 65 | prev_wrist = wrist_angle; 66 | } 67 | 68 | 69 | ros::Subscriber sub("joint_states", servo_cb); 70 | 71 | void setup(){ 72 | nh.getHardware()->setBaud(115200); 73 | nh.initNode(); 74 | nh.subscribe(sub); 75 | 76 | base.attach(8); 77 | shoulder.attach(9); 78 | elbow.attach(10); 79 | wrist.attach(11); 80 | gripper.attach(12); 81 | 82 | delay(1); 83 | base.write(90); 84 | shoulder.write(90); 85 | elbow.write(90); 86 | wrist.write(90); 87 | gripper.write(0); 88 | } 89 | 90 | void loop(){ 91 | nh.spinOnce(); 92 | } 93 | 94 | double radiansToDegrees(float position_radians) 95 | { 96 | 97 | position_radians = position_radians + 1.6; 98 | 99 | return position_radians * 57.2958; 100 | 101 | } 102 | -------------------------------------------------------------------------------- /circuit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/circuit.png -------------------------------------------------------------------------------- /moveit_pkg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(moveit_pkg) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | moveit_ros_planning 9 | moveit_ros_planning_interface 10 | moveit_core 11 | geometry_msgs 12 | 13 | ) 14 | 15 | catkin_package(CATKIN_DEPENDS 16 | roscpp 17 | moveit_ros_planning 18 | moveit_ros_planning_interface 19 | moveit_core 20 | geometry_msgs 21 | ) 22 | 23 | 24 | 25 | include_directories( 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | 29 | 30 | add_executable(move_group_node src/move_group_interface.cpp) 31 | add_dependencies(move_group_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 32 | target_link_libraries(move_group_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 33 | install(TARGETS move_group_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 34 | 35 | 36 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 37 | PATTERN "setup_assistant.launch" EXCLUDE) 38 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 39 | -------------------------------------------------------------------------------- /moveit_pkg/config/arduino_robot_arm.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /moveit_pkg/config/chomp_planning.yaml: -------------------------------------------------------------------------------- 1 | planning_time_limit: 10.0 2 | max_iterations: 200 3 | max_iterations_after_collision_free: 5 4 | smoothness_cost_weight: 0.1 5 | obstacle_cost_weight: 1.0 6 | learning_rate: 0.01 7 | smoothness_cost_velocity: 0.0 8 | smoothness_cost_acceleration: 1.0 9 | smoothness_cost_jerk: 0.0 10 | ridge_factor: 0.01 11 | use_pseudo_inverse: false 12 | pseudo_inverse_ridge_factor: 1e-4 13 | joint_update_limit: 0.1 14 | collision_clearence: 0.2 15 | collision_threshold: 0.07 16 | use_stochastic_descent: true 17 | enable_failure_recovery: true 18 | max_recovery_attempts: 5 -------------------------------------------------------------------------------- /moveit_pkg/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - base_joint 5 | - shoulder 6 | - elbow 7 | - wrist -------------------------------------------------------------------------------- /moveit_pkg/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | base_joint: 6 | has_velocity_limits: true 7 | max_velocity: 1 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | elbow: 11 | has_velocity_limits: true 12 | max_velocity: 1 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | shoulder: 16 | has_velocity_limits: true 17 | max_velocity: 1 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | wrist: 21 | has_velocity_limits: true 22 | max_velocity: 1 23 | has_acceleration_limits: false 24 | max_acceleration: 0 -------------------------------------------------------------------------------- /moveit_pkg/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /moveit_pkg/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBL: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | EST: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECE: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECE: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECE: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRT: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnect: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstar: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRT: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRM: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstar: 54 | type: geometric::PRMstar 55 | FMT: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMT: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | 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 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDST: 74 | type: geometric::PDST 75 | STRIDE: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | 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 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRT: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRT: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiEST: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjEST: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRM: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstar: 110 | type: geometric::LazyPRMstar 111 | SPARS: 112 | type: geometric::SPARS 113 | 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 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwo: 118 | type: geometric::SPARStwo 119 | 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 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | arm: 124 | default_planner_config: None 125 | planner_configs: 126 | - SBL 127 | - EST 128 | - LBKPIECE 129 | - BKPIECE 130 | - KPIECE 131 | - RRT 132 | - RRTConnect 133 | - RRTstar 134 | - TRRT 135 | - PRM 136 | - PRMstar 137 | - FMT 138 | - BFMT 139 | - PDST 140 | - STRIDE 141 | - BiTRRT 142 | - LBTRRT 143 | - BiEST 144 | - ProjEST 145 | - LazyPRM 146 | - LazyPRMstar 147 | - SPARS 148 | - SPARStwo -------------------------------------------------------------------------------- /moveit_pkg/config/ros_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Simulation settings for using moveit_sim_controllers 2 | moveit_sim_hw_interface: 3 | joint_model_group: arm 4 | joint_model_group_pose: home 5 | # Settings for ros_control_boilerplate control loop 6 | generic_hw_control_loop: 7 | loop_hz: 300 8 | cycle_time_error_threshold: 0.01 9 | # Settings for ros_control hardware interface 10 | hardware_interface: 11 | joints: 12 | - base_joint 13 | - shoulder 14 | - elbow 15 | - wrist 16 | sim_control_mode: 1 # 0: position, 1: velocity 17 | # Publish all joint states 18 | # Creates the /joint_states topic necessary in ROS 19 | joint_state_controller: 20 | type: joint_state_controller/JointStateController 21 | publish_rate: 50 22 | controller_list: 23 | - name: arm_controller 24 | action_ns: follow_joint_trajectory 25 | default: True 26 | type: FollowJointTrajectory 27 | joints: 28 | - base_joint 29 | - shoulder 30 | - elbow 31 | - wrist 32 | arm_controller: 33 | type: position_controllers/JointTrajectoryController 34 | joints: 35 | - base_joint 36 | - shoulder 37 | - elbow 38 | - wrist 39 | gains: 40 | base_joint: 41 | p: 100 42 | d: 1 43 | i: 1 44 | i_clamp: 1 45 | shoulder: 46 | p: 100 47 | d: 1 48 | i: 1 49 | i_clamp: 1 50 | elbow: 51 | p: 100 52 | d: 1 53 | i: 1 54 | i_clamp: 1 55 | wrist: 56 | p: 100 57 | d: 1 58 | i: 1 59 | i_clamp: 1 -------------------------------------------------------------------------------- /moveit_pkg/config/sensors_3d.yaml: -------------------------------------------------------------------------------- 1 | # The name of this file shouldn't be changed, or else the Setup Assistant won't detect it 2 | sensors: 3 | - {} -------------------------------------------------------------------------------- /moveit_pkg/launch/arduino_robot_arm_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /moveit_pkg/launch/arduino_robot_arm_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /moveit_pkg/launch/chomp_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /moveit_pkg/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /moveit_pkg/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | [move_group/fake_controller_joint_states] 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /moveit_pkg/launch/demo_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | [/joint_states] 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /moveit_pkg/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /moveit_pkg/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /moveit_pkg/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /moveit_pkg/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 32 | 33 | 34 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /moveit_pkg/launch/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /MotionPlanning1 8 | Splitter Ratio: 0.74256 9 | Tree Height: 664 10 | - Class: rviz/Help 11 | Name: Help 12 | - Class: rviz/Views 13 | Expanded: 14 | - /Current View1 15 | Name: Views 16 | Splitter Ratio: 0.5 17 | Visualization Manager: 18 | Class: "" 19 | Displays: 20 | - Alpha: 0.5 21 | Cell Size: 1 22 | Class: rviz/Grid 23 | Color: 160; 160; 164 24 | Enabled: true 25 | Line Style: 26 | Line Width: 0.03 27 | Value: Lines 28 | Name: Grid 29 | Normal Cell Count: 0 30 | Offset: 31 | X: 0 32 | Y: 0 33 | Z: 0 34 | Plane: XY 35 | Plane Cell Count: 10 36 | Reference Frame: 37 | Value: true 38 | - Class: moveit_rviz_plugin/MotionPlanning 39 | Enabled: true 40 | MoveIt_Goal_Tolerance: 0 41 | MoveIt_Planning_Time: 5 42 | MoveIt_Use_Constraint_Aware_IK: true 43 | MoveIt_Warehouse_Host: 127.0.0.1 44 | MoveIt_Warehouse_Port: 33829 45 | Name: MotionPlanning 46 | Planned Path: 47 | Links: 48 | base_bellow_link: 49 | Alpha: 1 50 | Show Axes: false 51 | Show Trail: false 52 | Value: true 53 | base_footprint: 54 | Alpha: 1 55 | Show Axes: false 56 | Show Trail: false 57 | Value: true 58 | base_link: 59 | Alpha: 1 60 | Show Axes: false 61 | Show Trail: false 62 | Value: true 63 | bl_caster_l_wheel_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | bl_caster_r_wheel_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | bl_caster_rotation_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | br_caster_l_wheel_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | br_caster_r_wheel_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | br_caster_rotation_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | double_stereo_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | fl_caster_l_wheel_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | fl_caster_r_wheel_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | fl_caster_rotation_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | fr_caster_l_wheel_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | fr_caster_r_wheel_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | fr_caster_rotation_link: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | head_mount_kinect_ir_link: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | head_mount_kinect_rgb_link: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | head_mount_link: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | head_mount_prosilica_link: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | head_pan_link: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | head_plate_frame: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | head_tilt_link: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | l_elbow_flex_link: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | l_forearm_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | l_forearm_roll_link: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | Value: true 178 | l_gripper_l_finger_link: 179 | Alpha: 1 180 | Show Axes: false 181 | Show Trail: false 182 | Value: true 183 | l_gripper_l_finger_tip_link: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | Value: true 188 | l_gripper_motor_accelerometer_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | l_gripper_palm_link: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | Value: true 198 | l_gripper_r_finger_link: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | l_gripper_r_finger_tip_link: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | l_shoulder_lift_link: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | Value: true 213 | l_shoulder_pan_link: 214 | Alpha: 1 215 | Show Axes: false 216 | Show Trail: false 217 | Value: true 218 | l_upper_arm_link: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | l_upper_arm_roll_link: 224 | Alpha: 1 225 | Show Axes: false 226 | Show Trail: false 227 | Value: true 228 | l_wrist_flex_link: 229 | Alpha: 1 230 | Show Axes: false 231 | Show Trail: false 232 | Value: true 233 | l_wrist_roll_link: 234 | Alpha: 1 235 | Show Axes: false 236 | Show Trail: false 237 | Value: true 238 | laser_tilt_mount_link: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | Value: true 243 | r_elbow_flex_link: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | Value: true 248 | r_forearm_link: 249 | Alpha: 1 250 | Show Axes: false 251 | Show Trail: false 252 | Value: true 253 | r_forearm_roll_link: 254 | Alpha: 1 255 | Show Axes: false 256 | Show Trail: false 257 | Value: true 258 | r_gripper_l_finger_link: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | Value: true 263 | r_gripper_l_finger_tip_link: 264 | Alpha: 1 265 | Show Axes: false 266 | Show Trail: false 267 | Value: true 268 | r_gripper_motor_accelerometer_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | Value: true 273 | r_gripper_palm_link: 274 | Alpha: 1 275 | Show Axes: false 276 | Show Trail: false 277 | Value: true 278 | r_gripper_r_finger_link: 279 | Alpha: 1 280 | Show Axes: false 281 | Show Trail: false 282 | Value: true 283 | r_gripper_r_finger_tip_link: 284 | Alpha: 1 285 | Show Axes: false 286 | Show Trail: false 287 | Value: true 288 | r_shoulder_lift_link: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | r_shoulder_pan_link: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | r_upper_arm_link: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | r_upper_arm_roll_link: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | r_wrist_flex_link: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | Value: true 313 | r_wrist_roll_link: 314 | Alpha: 1 315 | Show Axes: false 316 | Show Trail: false 317 | Value: true 318 | sensor_mount_link: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | Value: true 323 | torso_lift_link: 324 | Alpha: 1 325 | Show Axes: false 326 | Show Trail: false 327 | Value: true 328 | Loop Animation: true 329 | Robot Alpha: 0.5 330 | Show Robot Collision: false 331 | Show Robot Visual: true 332 | Show Trail: false 333 | State Display Time: 0.05 s 334 | Trajectory Topic: move_group/display_planned_path 335 | Planning Metrics: 336 | Payload: 1 337 | Show Joint Torques: false 338 | Show Manipulability: false 339 | Show Manipulability Index: false 340 | Show Weight Limit: false 341 | Planning Request: 342 | Colliding Link Color: 255; 0; 0 343 | Goal State Alpha: 1 344 | Goal State Color: 250; 128; 0 345 | Interactive Marker Size: 0 346 | Joint Violation Color: 255; 0; 255 347 | Planning Group: left_arm 348 | Query Goal State: true 349 | Query Start State: false 350 | Show Workspace: false 351 | Start State Alpha: 1 352 | Start State Color: 0; 255; 0 353 | Planning Scene Topic: move_group/monitored_planning_scene 354 | Robot Description: robot_description 355 | Scene Geometry: 356 | Scene Alpha: 1 357 | Scene Color: 50; 230; 50 358 | Scene Display Time: 0.2 359 | Show Scene Geometry: true 360 | Voxel Coloring: Z-Axis 361 | Voxel Rendering: Occupied Voxels 362 | Scene Robot: 363 | Attached Body Color: 150; 50; 150 364 | Links: 365 | base_bellow_link: 366 | Alpha: 1 367 | Show Axes: false 368 | Show Trail: false 369 | Value: true 370 | base_footprint: 371 | Alpha: 1 372 | Show Axes: false 373 | Show Trail: false 374 | Value: true 375 | base_link: 376 | Alpha: 1 377 | Show Axes: false 378 | Show Trail: false 379 | Value: true 380 | bl_caster_l_wheel_link: 381 | Alpha: 1 382 | Show Axes: false 383 | Show Trail: false 384 | Value: true 385 | bl_caster_r_wheel_link: 386 | Alpha: 1 387 | Show Axes: false 388 | Show Trail: false 389 | Value: true 390 | bl_caster_rotation_link: 391 | Alpha: 1 392 | Show Axes: false 393 | Show Trail: false 394 | Value: true 395 | br_caster_l_wheel_link: 396 | Alpha: 1 397 | Show Axes: false 398 | Show Trail: false 399 | Value: true 400 | br_caster_r_wheel_link: 401 | Alpha: 1 402 | Show Axes: false 403 | Show Trail: false 404 | Value: true 405 | br_caster_rotation_link: 406 | Alpha: 1 407 | Show Axes: false 408 | Show Trail: false 409 | Value: true 410 | double_stereo_link: 411 | Alpha: 1 412 | Show Axes: false 413 | Show Trail: false 414 | Value: true 415 | fl_caster_l_wheel_link: 416 | Alpha: 1 417 | Show Axes: false 418 | Show Trail: false 419 | Value: true 420 | fl_caster_r_wheel_link: 421 | Alpha: 1 422 | Show Axes: false 423 | Show Trail: false 424 | Value: true 425 | fl_caster_rotation_link: 426 | Alpha: 1 427 | Show Axes: false 428 | Show Trail: false 429 | Value: true 430 | fr_caster_l_wheel_link: 431 | Alpha: 1 432 | Show Axes: false 433 | Show Trail: false 434 | Value: true 435 | fr_caster_r_wheel_link: 436 | Alpha: 1 437 | Show Axes: false 438 | Show Trail: false 439 | Value: true 440 | fr_caster_rotation_link: 441 | Alpha: 1 442 | Show Axes: false 443 | Show Trail: false 444 | Value: true 445 | head_mount_kinect_ir_link: 446 | Alpha: 1 447 | Show Axes: false 448 | Show Trail: false 449 | Value: true 450 | head_mount_kinect_rgb_link: 451 | Alpha: 1 452 | Show Axes: false 453 | Show Trail: false 454 | Value: true 455 | head_mount_link: 456 | Alpha: 1 457 | Show Axes: false 458 | Show Trail: false 459 | Value: true 460 | head_mount_prosilica_link: 461 | Alpha: 1 462 | Show Axes: false 463 | Show Trail: false 464 | Value: true 465 | head_pan_link: 466 | Alpha: 1 467 | Show Axes: false 468 | Show Trail: false 469 | Value: true 470 | head_plate_frame: 471 | Alpha: 1 472 | Show Axes: false 473 | Show Trail: false 474 | Value: true 475 | head_tilt_link: 476 | Alpha: 1 477 | Show Axes: false 478 | Show Trail: false 479 | Value: true 480 | l_elbow_flex_link: 481 | Alpha: 1 482 | Show Axes: false 483 | Show Trail: false 484 | Value: true 485 | l_forearm_link: 486 | Alpha: 1 487 | Show Axes: false 488 | Show Trail: false 489 | Value: true 490 | l_forearm_roll_link: 491 | Alpha: 1 492 | Show Axes: false 493 | Show Trail: false 494 | Value: true 495 | l_gripper_l_finger_link: 496 | Alpha: 1 497 | Show Axes: false 498 | Show Trail: false 499 | Value: true 500 | l_gripper_l_finger_tip_link: 501 | Alpha: 1 502 | Show Axes: false 503 | Show Trail: false 504 | Value: true 505 | l_gripper_motor_accelerometer_link: 506 | Alpha: 1 507 | Show Axes: false 508 | Show Trail: false 509 | Value: true 510 | l_gripper_palm_link: 511 | Alpha: 1 512 | Show Axes: false 513 | Show Trail: false 514 | Value: true 515 | l_gripper_r_finger_link: 516 | Alpha: 1 517 | Show Axes: false 518 | Show Trail: false 519 | Value: true 520 | l_gripper_r_finger_tip_link: 521 | Alpha: 1 522 | Show Axes: false 523 | Show Trail: false 524 | Value: true 525 | l_shoulder_lift_link: 526 | Alpha: 1 527 | Show Axes: false 528 | Show Trail: false 529 | Value: true 530 | l_shoulder_pan_link: 531 | Alpha: 1 532 | Show Axes: false 533 | Show Trail: false 534 | Value: true 535 | l_upper_arm_link: 536 | Alpha: 1 537 | Show Axes: false 538 | Show Trail: false 539 | Value: true 540 | l_upper_arm_roll_link: 541 | Alpha: 1 542 | Show Axes: false 543 | Show Trail: false 544 | Value: true 545 | l_wrist_flex_link: 546 | Alpha: 1 547 | Show Axes: false 548 | Show Trail: false 549 | Value: true 550 | l_wrist_roll_link: 551 | Alpha: 1 552 | Show Axes: false 553 | Show Trail: false 554 | Value: true 555 | laser_tilt_mount_link: 556 | Alpha: 1 557 | Show Axes: false 558 | Show Trail: false 559 | Value: true 560 | r_elbow_flex_link: 561 | Alpha: 1 562 | Show Axes: false 563 | Show Trail: false 564 | Value: true 565 | r_forearm_link: 566 | Alpha: 1 567 | Show Axes: false 568 | Show Trail: false 569 | Value: true 570 | r_forearm_roll_link: 571 | Alpha: 1 572 | Show Axes: false 573 | Show Trail: false 574 | Value: true 575 | r_gripper_l_finger_link: 576 | Alpha: 1 577 | Show Axes: false 578 | Show Trail: false 579 | Value: true 580 | r_gripper_l_finger_tip_link: 581 | Alpha: 1 582 | Show Axes: false 583 | Show Trail: false 584 | Value: true 585 | r_gripper_motor_accelerometer_link: 586 | Alpha: 1 587 | Show Axes: false 588 | Show Trail: false 589 | Value: true 590 | r_gripper_palm_link: 591 | Alpha: 1 592 | Show Axes: false 593 | Show Trail: false 594 | Value: true 595 | r_gripper_r_finger_link: 596 | Alpha: 1 597 | Show Axes: false 598 | Show Trail: false 599 | Value: true 600 | r_gripper_r_finger_tip_link: 601 | Alpha: 1 602 | Show Axes: false 603 | Show Trail: false 604 | Value: true 605 | r_shoulder_lift_link: 606 | Alpha: 1 607 | Show Axes: false 608 | Show Trail: false 609 | Value: true 610 | r_shoulder_pan_link: 611 | Alpha: 1 612 | Show Axes: false 613 | Show Trail: false 614 | Value: true 615 | r_upper_arm_link: 616 | Alpha: 1 617 | Show Axes: false 618 | Show Trail: false 619 | Value: true 620 | r_upper_arm_roll_link: 621 | Alpha: 1 622 | Show Axes: false 623 | Show Trail: false 624 | Value: true 625 | r_wrist_flex_link: 626 | Alpha: 1 627 | Show Axes: false 628 | Show Trail: false 629 | Value: true 630 | r_wrist_roll_link: 631 | Alpha: 1 632 | Show Axes: false 633 | Show Trail: false 634 | Value: true 635 | sensor_mount_link: 636 | Alpha: 1 637 | Show Axes: false 638 | Show Trail: false 639 | Value: true 640 | torso_lift_link: 641 | Alpha: 1 642 | Show Axes: false 643 | Show Trail: false 644 | Value: true 645 | Robot Alpha: 0.5 646 | Show Scene Robot: true 647 | Value: true 648 | Enabled: true 649 | Global Options: 650 | Background Color: 48; 48; 48 651 | Fixed Frame: /base 652 | Name: root 653 | Tools: 654 | - Class: rviz/Interact 655 | Hide Inactive Objects: true 656 | - Class: rviz/MoveCamera 657 | - Class: rviz/Select 658 | Value: true 659 | Views: 660 | Current: 661 | Class: rviz/XYOrbit 662 | Distance: 2.9965 663 | Focal Point: 664 | X: 0.113567 665 | Y: 0.10592 666 | Z: 2.23518e-07 667 | Name: Current View 668 | Near Clip Distance: 0.01 669 | Pitch: 0.509797 670 | Target Frame: /base 671 | Value: XYOrbit (rviz) 672 | Yaw: 5.65995 673 | Saved: ~ 674 | Window Geometry: 675 | Displays: 676 | collapsed: false 677 | Height: 1337 678 | Help: 679 | collapsed: false 680 | Hide Left Dock: false 681 | Hide Right Dock: false 682 | Motion Planning: 683 | collapsed: false 684 | QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 685 | Views: 686 | collapsed: false 687 | Width: 1828 688 | X: 459 689 | Y: -243 690 | -------------------------------------------------------------------------------- /moveit_pkg/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /moveit_pkg/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /moveit_pkg/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /moveit_pkg/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /moveit_pkg/launch/ros_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /moveit_pkg/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /moveit_pkg/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /moveit_pkg/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /moveit_pkg/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /moveit_pkg/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /moveit_pkg/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /moveit_pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_pkg 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the arduino_robot_arm with the MoveIt! Motion Planning Framework 7 | 8 | Smart Methods 9 | Smart Methods 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit/issues 15 | https://github.com/ros-planning/moveit 16 | 17 | catkin 18 | 19 | roscpp 20 | roscpp 21 | moveit_ros_planning 22 | moveit_ros_planning 23 | moveit_ros_planning_interface 24 | moveit_ros_planning_interface 25 | moveit_core 26 | moveit_core 27 | geometry_msgs 28 | geometry_msgs 29 | 30 | moveit_ros_move_group 31 | moveit_fake_controller_manager 32 | moveit_kinematics 33 | moveit_planners_ompl 34 | moveit_ros_visualization 35 | moveit_setup_assistant 36 | joint_state_publisher 37 | robot_state_publisher 38 | xacro 39 | 40 | 41 | robot_arm_pkg 42 | robot_arm_pkg 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /moveit_pkg/scripts/color_thresholding.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import cv2 3 | import numpy as np 4 | 5 | def nothing(pos): 6 | pass 7 | 8 | cap=cv2.VideoCapture(1) 9 | 10 | cv2.namedWindow('Thresholds') 11 | cv2.createTrackbar('LH','Thresholds',0,255, nothing) 12 | cv2.createTrackbar('LS','Thresholds',0,255, nothing) 13 | cv2.createTrackbar('LV','Thresholds',0,255, nothing) 14 | cv2.createTrackbar('UH','Thresholds',255,255, nothing) 15 | cv2.createTrackbar('US','Thresholds',255,255, nothing) 16 | cv2.createTrackbar('UV','Thresholds',255,255, nothing) 17 | 18 | 19 | while(1): 20 | _, img = cap.read() 21 | 22 | #converting frame(img i.e BGR) to HSV (hue-saturation-value) 23 | 24 | hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV) 25 | 26 | lh=cv2.getTrackbarPos('LH','Thresholds') 27 | ls=cv2.getTrackbarPos('LS','Thresholds') 28 | lv=cv2.getTrackbarPos('LV','Thresholds') 29 | 30 | uh=cv2.getTrackbarPos('UH','Thresholds') 31 | us=cv2.getTrackbarPos('US','Thresholds') 32 | uv=cv2.getTrackbarPos('UV','Thresholds') 33 | 34 | #defining the Range of color 35 | color_lower=np.array([lh,ls,lv],np.uint8) 36 | color_upper=np.array([uh,us,uv],np.uint8) 37 | 38 | 39 | #finding the range of color in the image 40 | 41 | color=cv2.inRange(hsv,color_lower,color_upper) 42 | 43 | #Morphological transformation, Dilation 44 | kernal = np.ones((5 ,5), "uint8") 45 | 46 | 47 | color=cv2.dilate(color,kernal) 48 | 49 | 50 | cv2.imshow("Color",color) 51 | cv2.imshow("Original Image",img) 52 | 53 | if cv2.waitKey(1)== ord('q'): 54 | break 55 | 56 | cap.release() 57 | cv2.destroyAllWindows() 58 | -------------------------------------------------------------------------------- /moveit_pkg/scripts/get_pose_openCV.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from std_msgs.msg import String 5 | from geometry_msgs.msg import Pose 6 | 7 | import sys 8 | import cv2 9 | import numpy as np 10 | 11 | 12 | cap=cv2.VideoCapture(0) 13 | 14 | 15 | pub = rospy.Publisher('direction', Pose, queue_size=10) 16 | rospy.init_node('vision', anonymous=True) 17 | rate = rospy.Rate(10) # 10hz 18 | rospy.loginfo("Direction is published ........ # see /direction topic") 19 | 20 | 21 | 22 | positionX = -0.0023734011670250126 23 | positionY = 0.026825106858739834 24 | positionZ = 0.32402280551708873 25 | 26 | orientationX = 0.4966690471541269 27 | orientationY = -0.4966708043538031 28 | orientationZ = -0.5033089303160498 29 | orientationW = -0.5033071531040186 30 | 31 | 32 | while(1): 33 | _, img = cap.read() 34 | 35 | #converting frame(img i.e BGR) to HSV (hue-saturation-value) 36 | 37 | hsv=cv2.cvtColor(img,cv2.COLOR_BGR2HSV) 38 | blue_lower=np.array([94,123,46],np.uint8) 39 | blue_upper=np.array([125,255,255],np.uint8) 40 | 41 | 42 | blue=cv2.inRange(hsv,blue_lower,blue_upper) 43 | 44 | #Morphological transformation, Dilation 45 | kernal = np.ones((5 ,5), "uint8") 46 | 47 | 48 | blue=cv2.dilate(blue,kernal) 49 | 50 | 51 | (_,contours,hierarchy)=cv2.findContours(blue,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE) 52 | 53 | 54 | if len(contours)>0: 55 | contour= max(contours,key=cv2.contourArea) 56 | area = cv2.contourArea(contour) 57 | if area>800: 58 | x,y,w,h = cv2.boundingRect(contour) 59 | 60 | ss = 'x:' + str(x) + ', y:' + str(y) 61 | 62 | cv2.putText(img,ss,(x-20,y-5),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255),1,cv2.LINE_AA) 63 | 64 | OldRangeX = (640 - 0) 65 | NewRangeX = (0.2 - (-0.2)) 66 | positionX = (((x - 0) * NewRangeX) / OldRangeX) + (-0.2) 67 | 68 | OldRangeY = (480 - 0) 69 | NewRangeY = (0.2 - (-0.2)) 70 | positionY = (((y - 0) * NewRangeY) / OldRangeY) + (-0.2) 71 | 72 | positionZ = 0.0087 73 | 74 | orientationX = 0.0 75 | orientationY = 0.0 76 | orientationZ = 0.0 77 | orientationW = 1.0 78 | 79 | cv2.imshow("Mask",blue) 80 | cv2.imshow("Color Tracking",img) 81 | k = cv2.waitKey(10) & 0xff # Press 'ESC' for exiting video 82 | if k == 27: 83 | break 84 | 85 | 86 | if not rospy.is_shutdown(): 87 | 88 | p = Pose() 89 | p.position.x = positionX 90 | p.position.y = positionY 91 | p.position.z = positionZ 92 | p.orientation.x = orientationX 93 | p.orientation.y = orientationY 94 | p.orientation.z = orientationZ 95 | p.orientation.w = orientationW 96 | 97 | pub.publish(p) 98 | rate.sleep() 99 | 100 | cap.release() 101 | cv2.destroyAllWindows() 102 | 103 | -------------------------------------------------------------------------------- /moveit_pkg/src/move_group_interface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | //MOVE IT 5 | #include 6 | #include 7 | 8 | float positionX = -0.0023734011670250126; 9 | float positionY = 0.026825106858739834; 10 | float positionZ = 0.32402280551708873; 11 | 12 | float orientationX = 0.4966690471541269; 13 | float orientationY = -0.4966708043538031; 14 | float orientationZ = -0.5033089303160498; 15 | float orientationW = -0.5033071531040186; 16 | 17 | void chatterCallback(const geometry_msgs::Pose::ConstPtr& msg) 18 | { 19 | float new_positionX = msg->position.x; 20 | float new_positionY = msg->position.y; 21 | float new_positionZ = msg->position.z; 22 | 23 | ROS_INFO("saved_x: [%.9f], new:[%.9f]", positionX, new_positionX); 24 | ROS_INFO("saved_y: [%.9f], new [%.9f]", positionY, new_positionY); 25 | ROS_INFO("saved_z: [%.9f], new [%.9f]", positionZ, new_positionZ); 26 | 27 | 28 | if (new_positionX!=positionX ) 29 | { 30 | 31 | ROS_INFO("new pose"); 32 | 33 | //planning group that we would like to control 34 | moveit::planning_interface::MoveGroupInterface group("arm"); 35 | 36 | 37 | //raw pointers are used to refer to the planning group for improved performance 38 | const robot_state::JointModelGroup* joint_model_group = group.getCurrentState()->getJointModelGroup("arm"); 39 | group.setEndEffectorLink("arm3"); 40 | group.setPoseReferenceFrame("base"); 41 | group.setPlannerId("base"); 42 | group.setNumPlanningAttempts(50); 43 | group.setPlanningTime(50.0); 44 | group.allowReplanning(true); 45 | group.setGoalJointTolerance(0.0001); 46 | group.setGoalPositionTolerance(0.0001); 47 | group.setGoalOrientationTolerance(0.001); 48 | //group.setNamedTarget("random"); 49 | group.setRandomTarget(); 50 | group.move(); // WORKS FINE :) 51 | 52 | // CUSTOM PLANNING 53 | geometry_msgs::Pose target_pose1; 54 | //NOTE: THIS IS THE VALID POSE FROM RANDOM NODE 55 | 56 | 57 | target_pose1.orientation.w = msg->orientation.w; 58 | target_pose1.orientation.x = msg->orientation.x; 59 | target_pose1.orientation.y = msg->orientation.y; 60 | target_pose1.orientation.z = msg->orientation.z; 61 | 62 | target_pose1.position.x = msg->position.x; 63 | target_pose1.position.y = msg->position.y; 64 | target_pose1.position.z = msg->position.z; 65 | 66 | group.setStartStateToCurrentState(); 67 | group.setPoseTarget(target_pose1,"arm3"); 68 | 69 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 70 | moveit_msgs::MotionPlanRequest response; 71 | 72 | group.plan(my_plan); 73 | group.execute(my_plan); 74 | 75 | 76 | ROS_INFO("pose updated"); 77 | 78 | 79 | 80 | 81 | positionX = msg->position.x; 82 | positionY = msg->position.y; 83 | positionZ = msg->position.z; 84 | orientationX = msg->orientation.x; 85 | orientationY = msg->orientation.y; 86 | orientationZ = msg->orientation.z; 87 | orientationW = msg->orientation.w; 88 | 89 | } 90 | 91 | 92 | } 93 | 94 | 95 | 96 | int main(int argc, char **argv) 97 | { 98 | 99 | ros::init(argc, argv, "move_group_interface"); 100 | ros::NodeHandle nh; 101 | ROS_INFO("initilize !"); 102 | 103 | 104 | // define user callback queue 105 | ros::CallbackQueue string_queue; 106 | // create options for subscriber and pass pointer to our custom queue 107 | ros::SubscribeOptions ops = 108 | ros::SubscribeOptions::create( 109 | "/direction", // topic name 110 | 1000, // queue length 111 | chatterCallback, // callback 112 | ros::VoidPtr(), // tracked object, we don't need one thus NULL 113 | &string_queue // pointer to callback queue object 114 | ); 115 | // subscribe 116 | ros::Subscriber sub2 = nh.subscribe(ops); 117 | 118 | 119 | ros::AsyncSpinner spinner(1, &string_queue); 120 | spinner.start(); 121 | 122 | 123 | 124 | 125 | ros::Rate r(10); // 10 hz 126 | while (ros::ok()) 127 | { 128 | ros::spinOnce(); 129 | 130 | r.sleep(); 131 | } 132 | 133 | return 0; 134 | 135 | } 136 | -------------------------------------------------------------------------------- /positions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/positions.png -------------------------------------------------------------------------------- /robot_arm_pkg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(robot_arm_pkg) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a exec_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES robot_arm_pkg 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/robot_arm_pkg.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/robot_arm_pkg_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # catkin_install_python(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables for installation 168 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 169 | # install(TARGETS ${PROJECT_NAME}_node 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark libraries for installation 174 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 175 | # install(TARGETS ${PROJECT_NAME} 176 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 179 | # ) 180 | 181 | ## Mark cpp header files for installation 182 | # install(DIRECTORY include/${PROJECT_NAME}/ 183 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | # FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | # ) 187 | 188 | ## Mark other files for installation (e.g. launch and bag files, etc.) 189 | # install(FILES 190 | # # myfile1 191 | # # myfile2 192 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 193 | # ) 194 | 195 | ############# 196 | ## Testing ## 197 | ############# 198 | 199 | ## Add gtest based cpp test target and link libraries 200 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_arm_pkg.cpp) 201 | # if(TARGET ${PROJECT_NAME}-test) 202 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 203 | # endif() 204 | 205 | ## Add folders to be run by python nosetests 206 | # catkin_add_nosetests(test) 207 | 208 | catkin_install_python(PROGRAMS scripts/joint_states_to_gazebo.py 209 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 210 | -------------------------------------------------------------------------------- /robot_arm_pkg/config/ros_controllers.yaml: -------------------------------------------------------------------------------- 1 | 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | 6 | 7 | arm_controller: 8 | type: position_controllers/JointTrajectoryController 9 | joints: 10 | - base_joint 11 | - shoulder 12 | - elbow 13 | - wrist 14 | gains: 15 | base_joint: 16 | p: 100 17 | d: 1 18 | i: 1 19 | i_clamp: 1 20 | shoulder: 21 | p: 100 22 | d: 1 23 | i: 1 24 | i_clamp: 1 25 | elbow: 26 | p: 100 27 | d: 1 28 | i: 1 29 | i_clamp: 1 30 | wrist: 31 | p: 100 32 | d: 1 33 | i: 1 34 | i_clamp: 1 35 | -------------------------------------------------------------------------------- /robot_arm_pkg/launch/check_motors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /robot_arm_pkg/launch/check_motors_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robot_arm_pkg/launch/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Axes1 10 | - /RobotModel1 11 | - /RobotModel1/Links1 12 | Splitter Ratio: 0.5 13 | Tree Height: 330 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/Axes 57 | Enabled: true 58 | Length: 1 59 | Name: Axes 60 | Radius: 0.00500000007 61 | Reference Frame: 62 | Value: true 63 | - Alpha: 0.5 64 | Class: rviz/RobotModel 65 | Collision Enabled: false 66 | Enabled: true 67 | Links: 68 | All Links Enabled: true 69 | Expand Joint Details: false 70 | Expand Link Details: true 71 | Expand Tree: false 72 | Link Tree Style: Links in Alphabetic Order 73 | arm1: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | arm2: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | base: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | gripper: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | waist: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | Name: RobotModel 99 | Robot Description: robot_description 100 | TF Prefix: "" 101 | Update Interval: 0 102 | Value: true 103 | Visual Enabled: true 104 | Enabled: true 105 | Global Options: 106 | Background Color: 48; 48; 48 107 | Default Light: true 108 | Fixed Frame: base 109 | Frame Rate: 30 110 | Name: root 111 | Tools: 112 | - Class: rviz/Interact 113 | Hide Inactive Objects: true 114 | - Class: rviz/MoveCamera 115 | - Class: rviz/Select 116 | - Class: rviz/FocusCamera 117 | - Class: rviz/Measure 118 | - Class: rviz/SetInitialPose 119 | Topic: /initialpose 120 | - Class: rviz/SetGoal 121 | Topic: /move_base_simple/goal 122 | - Class: rviz/PublishPoint 123 | Single click: true 124 | Topic: /clicked_point 125 | Value: true 126 | Views: 127 | Current: 128 | Class: rviz/Orbit 129 | Distance: 4.18737 130 | Enable Stereo Rendering: 131 | Stereo Eye Separation: 0.0599999987 132 | Stereo Focal Distance: 1 133 | Swap Stereo Eyes: false 134 | Value: false 135 | Focal Point: 136 | X: -1.2241 137 | Y: -2.4556 138 | Z: -1.5747 139 | Focal Shape Fixed Size: true 140 | Focal Shape Size: 0.0500000007 141 | Invert Z Axis: false 142 | Name: Current View 143 | Near Clip Distance: 0.00999999978 144 | Pitch: 0.575398088 145 | Target Frame: 146 | Value: Orbit (rviz) 147 | Yaw: 1.12539792 148 | Saved: ~ 149 | Window Geometry: 150 | Displays: 151 | collapsed: false 152 | Height: 611 153 | Hide Left Dock: false 154 | Hide Right Dock: false 155 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000001d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d9000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001d9000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004000000003efc0100000002fb0000000800540069006d00650100000000000004000000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000017b000001d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 156 | Selection: 157 | collapsed: false 158 | Time: 159 | collapsed: false 160 | Tool Properties: 161 | collapsed: false 162 | Views: 163 | collapsed: false 164 | Width: 1024 165 | X: 65 166 | Y: 24 167 | -------------------------------------------------------------------------------- /robot_arm_pkg/launch/gazebo_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /robot_arm_pkg/launch/rviz_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /robot_arm_pkg/launch/urdf_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 275 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Attached Body Color: 150; 50; 150 54 | Class: moveit_rviz_plugin/RobotState 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | arm1: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | arm2: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | base: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | gripper: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | waist: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | Name: RobotState 89 | Robot Alpha: 1 90 | Robot Description: robot_description 91 | Robot State Topic: display_robot_state 92 | Show All Links: true 93 | Show Highlights: true 94 | Value: true 95 | Visual Enabled: true 96 | Enabled: true 97 | Global Options: 98 | Background Color: 48; 48; 48 99 | Default Light: true 100 | Fixed Frame: map 101 | Frame Rate: 30 102 | Name: root 103 | Tools: 104 | - Class: rviz/Interact 105 | Hide Inactive Objects: true 106 | - Class: rviz/MoveCamera 107 | - Class: rviz/Select 108 | - Class: rviz/FocusCamera 109 | - Class: rviz/Measure 110 | - Class: rviz/SetInitialPose 111 | Topic: /initialpose 112 | - Class: rviz/SetGoal 113 | Topic: /move_base_simple/goal 114 | - Class: rviz/PublishPoint 115 | Single click: true 116 | Topic: /clicked_point 117 | Value: true 118 | Views: 119 | Current: 120 | Class: rviz/Orbit 121 | Distance: 1.27474558 122 | Enable Stereo Rendering: 123 | Stereo Eye Separation: 0.0599999987 124 | Stereo Focal Distance: 1 125 | Swap Stereo Eyes: false 126 | Value: false 127 | Focal Point: 128 | X: -0.035450682 129 | Y: -0.042124223 130 | Z: 0.0626569763 131 | Focal Shape Fixed Size: true 132 | Focal Shape Size: 0.0500000007 133 | Invert Z Axis: false 134 | Name: Current View 135 | Near Clip Distance: 0.00999999978 136 | Pitch: 0.5203982 137 | Target Frame: 138 | Value: Orbit (rviz) 139 | Yaw: 0.805397987 140 | Saved: ~ 141 | Window Geometry: 142 | Displays: 143 | collapsed: false 144 | Height: 556 145 | Hide Left Dock: false 146 | Hide Right Dock: false 147 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000001a2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001bd00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001a2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001a2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000001a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 148 | Selection: 149 | collapsed: false 150 | Time: 151 | collapsed: false 152 | Tool Properties: 153 | collapsed: false 154 | Views: 155 | collapsed: false 156 | Width: 1200 157 | X: 65 158 | Y: 24 159 | -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Arm 01.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Arm 01.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Arm 02.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Arm 02.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Base.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Cube.002.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Cube.002.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Gripper.001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Gripper.001.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Gripper.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Waist.001.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Waist.001.stl -------------------------------------------------------------------------------- /robot_arm_pkg/meshes/stl/Waist.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/smart-methods/arduino_robot_arm/66b02138582698ea5c89d9b5240bff288d8083b0/robot_arm_pkg/meshes/stl/Waist.stl -------------------------------------------------------------------------------- /robot_arm_pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_arm_pkg 4 | 0.0.0 5 | The robot_arm_pkg package 6 | 7 | 8 | 9 | 10 | ros 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | controller_manager 54 | joint_state_controller 55 | robot_state_publisher 56 | urdf 57 | 58 | controller_manager 59 | joint_state_controller 60 | robot_state_publisher 61 | urdf 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /robot_arm_pkg/scripts/joint_states_to_gazebo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from sensor_msgs.msg import JointState 5 | from std_msgs.msg import Header 6 | from trajectory_msgs.msg import JointTrajectory 7 | from trajectory_msgs.msg import JointTrajectoryPoint 8 | 9 | 10 | def callback(data): 11 | rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.position) 12 | pub = rospy.Publisher('arm_controller/command', JointTrajectory, queue_size=10) 13 | 14 | joints_str = JointTrajectory() 15 | joints_str.header = Header() 16 | joints_str.header.stamp = rospy.Time.now() 17 | joints_str.joint_names = ['base_joint', 'shoulder', 'elbow', 'wrist'] 18 | point=JointTrajectoryPoint() 19 | point.positions = [data.position[0], data.position[1], data.position[2], data.position[3]] 20 | point.time_from_start = rospy.Duration(2) 21 | joints_str.points.append(point) 22 | 23 | pub.publish(joints_str) 24 | rospy.loginfo("position updated") 25 | 26 | 27 | 28 | def listener(): 29 | rospy.init_node('states', anonymous=True) 30 | rospy.Subscriber("joint_states", JointState, callback) 31 | 32 | rospy.spin() 33 | 34 | 35 | 36 | if __name__ == '__main__': 37 | try: 38 | listener() 39 | except rospy.ROSInterruptException: 40 | pass 41 | -------------------------------------------------------------------------------- /robot_arm_pkg/urdf/arduino_robot_arm.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | transmission_interface/SimpleTransmission 146 | 147 | hardware_interface/PositionJointInterface 148 | 149 | 150 | hardware_interface/PositionJointInterface 151 | 1 152 | 153 | 154 | 155 | transmission_interface/SimpleTransmission 156 | 157 | hardware_interface/PositionJointInterface 158 | 159 | 160 | hardware_interface/PositionJointInterface 161 | 1 162 | 163 | 164 | 165 | transmission_interface/SimpleTransmission 166 | 167 | hardware_interface/PositionJointInterface 168 | 169 | 170 | hardware_interface/PositionJointInterface 171 | 1 172 | 173 | 174 | 175 | transmission_interface/SimpleTransmission 176 | 177 | hardware_interface/PositionJointInterface 178 | 179 | 180 | hardware_interface/PositionJointInterface 181 | 1 182 | 183 | 184 | 185 | 186 | / 187 | 188 | 189 | 190 | --------------------------------------------------------------------------------