├── srv ├── kpkv_msg.srv~ ├── pool_claim_msg.srv ├── kpkv_msg.srv └── upper_boundary_change.srv ├── gazebo_screenshot.jpg ├── msg ├── scara_upper_boundary.msg └── cylinder_blocks_poses.msg ├── launch ├── initialize.world ├── scara_robot_controller.launch └── initialize.launch ├── urdf ├── readme.txt ├── red_cylinder.urdf ├── blue_cylinder.urdf ├── conveyor_belt.urdf ├── scara_robot_right.urdf └── scara_robot_left.urdf ├── config └── scara_robot_left_control.yaml ├── action └── scara_gripper.action ├── CMakeLists.txt ├── package.xml ├── README.md └── src ├── scara_upper_boundary_maintainer.cpp ├── cylinder_active_pool.cpp ├── cylinder_blocks_poses_publisher.cpp ├── cylinder_blocks_spawner.cpp ├── scara_joint_controller.cpp ├── scara_gripper_action_server.cpp ├── scara_left_motion_planner.cpp └── scara_right_motion_planner.cpp /srv/kpkv_msg.srv~: -------------------------------------------------------------------------------- 1 | string joint_name 2 | float64 kp 3 | float64 kv 4 | --- 5 | bool setting_is_done 6 | -------------------------------------------------------------------------------- /gazebo_screenshot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangliu28/two_scara_collaboration/HEAD/gazebo_screenshot.jpg -------------------------------------------------------------------------------- /srv/pool_claim_msg.srv: -------------------------------------------------------------------------------- 1 | # service message to claim some cylinder from active pool 2 | 3 | int8 cylinder_index 4 | --- 5 | bool cylinder_claimed 6 | 7 | -------------------------------------------------------------------------------- /srv/kpkv_msg.srv: -------------------------------------------------------------------------------- 1 | # service message for pd parameter tuning of the joint controller 2 | 3 | float64 kp 4 | float64 kv 5 | --- 6 | bool setting_is_done 7 | 8 | -------------------------------------------------------------------------------- /msg/scara_upper_boundary.msg: -------------------------------------------------------------------------------- 1 | # message as a state machine to monitor upper motion boundary for collision avoidance 2 | # publish for topics 3 | 4 | bool in_action # whether it goes into the upper area of the conveyor belt 5 | float64 upper_boundary # the upper boundary data 6 | 7 | 8 | -------------------------------------------------------------------------------- /msg/cylinder_blocks_poses.msg: -------------------------------------------------------------------------------- 1 | # message type to describe 3-D position of the cylinder blocks 2 | # variable length array, length decided by topic /current_cylinder_blocks 3 | # to be published as a topic 4 | 5 | float64[] x # x coordinate in the world 6 | float64[] y # y coordinate in the world 7 | float64[] z # z coordinate in the world 8 | 9 | -------------------------------------------------------------------------------- /launch/initialize.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 11 | 12 | model://sun 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /srv/upper_boundary_change.srv: -------------------------------------------------------------------------------- 1 | # service message to change published data in upper boundary topics 2 | 3 | bool in_action_change # decide if in_action is to change or remain same 4 | bool in_action # whether it goes into the upper area of the conveyor belt 5 | bool upper_boundary_change # decide if upper_boundary is to change or remain same 6 | float64 upper_boundary # the upper boundary data 7 | --- 8 | bool change_is_done 9 | 10 | -------------------------------------------------------------------------------- /urdf/readme.txt: -------------------------------------------------------------------------------- 1 | 2 | URDF can not specify the pose of the robot itself within a world. It is also not a universal description format since it cannot specify joint loops (parallel linkages), and it lacks friction and other properties. Additionally, it cannot specify things that are not robots, such as lights, heightmaps, etc. 3 | 4 | SDF has been tried to specify friction in the model, but it turns out friction can be determined with a gazebo plugin. Check the urdf file of conveyor belt for details. 5 | 6 | 7 | -------------------------------------------------------------------------------- /config/scara_robot_left_control.yaml: -------------------------------------------------------------------------------- 1 | scara_robot_left: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Position Controllers --------------------------------------- 8 | joint1_position_controller: 9 | type: effort_controllers/JointPositionController 10 | joint: rotation1 11 | pid: {p: 100.0, i: 0.01, d: 10.0} 12 | joint2_position_controller: 13 | type: effort_controllers/JointPositionController 14 | joint: rotation2 15 | pid: {p: 100.0, i: 0.01, d: 10.0} -------------------------------------------------------------------------------- /action/scara_gripper.action: -------------------------------------------------------------------------------- 1 | # action server message for the gripper control 2 | # motion of the gripper includes going up and down, grapsing and releasing 3 | 4 | 5 | # goal definition 6 | # whether commanding the gripper in high or low position 7 | # -1 is up 8 | # 0 keeps status 9 | # 1 is down (means stretch out the gripper for some task) 10 | int8 up_down_left 11 | int8 up_down_right 12 | # whether the gripper is grasping or releasing, grasp is true 13 | # -1 is release 14 | # 0 keeps status 15 | # 1 is grasp 16 | int8 grasp_release_left 17 | int8 grasp_release_right 18 | --- 19 | # result definition 20 | int32 result 21 | --- 22 | # feedback 23 | int32 fdbk 24 | 25 | 26 | -------------------------------------------------------------------------------- /urdf/red_cylinder.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 26 | 27 | 28 | 29 | 30 | Gazebo/Red 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /urdf/blue_cylinder.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 26 | 27 | 28 | 29 | 30 | Gazebo/Blue 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /launch/scara_robot_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /urdf/conveyor_belt.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 | 37 | 38 | 39 | 40 | 41 | 0 42 | 0 43 | Gazebo/FlatBlack 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(two_scara_collaboration) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | 6 | #uncomment next line to use OpenCV library 7 | #find_package(OpenCV REQUIRED) 8 | 9 | #uncomment the next line to use the point-cloud library 10 | #find_package(PCL 1.7 REQUIRED) 11 | 12 | #uncomment the following 4 lines to use the Eigen library 13 | #find_package(cmake_modules REQUIRED) 14 | #find_package(Eigen3 REQUIRED) 15 | #include_directories(${EIGEN3_INCLUDE_DIR}) 16 | #add_definitions(${EIGEN_DEFINITIONS}) 17 | 18 | catkin_simple() 19 | 20 | # example boost usage 21 | # find_package(Boost REQUIRED COMPONENTS system thread) 22 | 23 | # C++0x support - not quite the same as final C++11! 24 | # use carefully; can interfere with point-cloud library 25 | # SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 26 | 27 | # Libraries: uncomment the following and edit arguments to create a new library 28 | # cs_add_library(my_lib src/my_lib.cpp) 29 | 30 | # Executables: uncomment the following and edit arguments to compile new nodes 31 | # may add more of these lines for more nodes from the same package 32 | # cs_add_executable(example src/example.cpp) 33 | cs_add_executable(cylinder_blocks_spawner src/cylinder_blocks_spawner.cpp) 34 | cs_add_executable(cylinder_blocks_poses_publisher src/cylinder_blocks_poses_publisher.cpp) 35 | cs_add_executable(scara_gripper_action_server src/scara_gripper_action_server.cpp) 36 | cs_add_executable(scara_joint_controller src/scara_joint_controller.cpp) 37 | cs_add_executable(cylinder_active_pool src/cylinder_active_pool.cpp) 38 | cs_add_executable(scara_upper_boundary_maintainer src/scara_upper_boundary_maintainer.cpp) 39 | cs_add_executable(scara_left_motion_planner src/scara_left_motion_planner.cpp) 40 | cs_add_executable(scara_right_motion_planner src/scara_right_motion_planner.cpp) 41 | 42 | #the following is required, if desire to link a node in this package with a library created in this same package 43 | # edit the arguments to reference the named node and named library within this package 44 | # target_link_library(example my_lib) 45 | 46 | cs_install() 47 | cs_export() 48 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | two_scara_collaboration 4 | 0.0.0 5 | The two_scara_collaboration package 6 | 7 | 8 | 9 | 10 | yang 11 | 12 | 13 | 14 | 15 | TODO 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | message_generation 35 | 36 | 37 | 38 | message_runtime 39 | 40 | 41 | catkin 42 | catkin_simple 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | actionlib 48 | actionlib 49 | 50 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /launch/initialize.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 | 29 | 30 | 31 | 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # two_scara_collaboration 2 | 3 | Simulation of two SCARA robots collaboratively sorting items from a conveyor belt with collision avoidance and task distribution. 4 | 5 | Check the video demonstration here: [https://youtu.be/I47J0p1j1zU](https://youtu.be/I47J0p1j1zU) 6 | 7 | ## Test command 8 | ``` 9 | roslaunch two_scara_collaboration initialize.launch 10 | ``` 11 | Gazebo simulator is paused when initialized, in case the arms of the two scara robots appear at same location and bounce away each other. Click start button to start. 12 | 13 | Start the two independant motion planners 14 | ``` 15 | rosrun two_scara_collaboration scara_left_motion_planner 16 | ``` 17 | ``` 18 | rosrun two_scara_collaboration scara_right_motion_planner 19 | ``` 20 | 21 | 22 | ## Progress (May 7, 2016) 23 | Switch the joint control method from off-the-shelf ros_control package to self-defined PD controller. 24 | 25 | Collision avoidance using a further simplified algorithm to dynamically allocate each robot's workspace. Task distribution follows a first come, first get principle. One important thing is that, the motion planners are independant. One planner for one SCARA robot. And they are pretty robust observed from the video. 26 | 27 | Some parameters, like time and exerted force or torque can be further tuned, to get better motion performance. 28 | 29 | 30 | ## Progress and problems (May 3, 2016) 31 | The scara robots, conveyor belt and cylinder blocks were modeled in urdf. 32 | 33 | A spawner node can automatically spawn cylinder blocks and let them slide on conveyor with an initial speed. 34 | 35 | A cylinder blocks position publisher node was added to publish the 3-D position of all current cylinders. 36 | 37 | A gripper control action server node can keep gripper at desired location, and receiving command on the gripper action. 38 | 39 | Steps not finished in the plan: 40 | 41 | joint position controller of scara (problem with ros_control) 42 | 43 | inverse kinematics of scara robot 44 | 45 | collision avoidance 46 | 47 | task distribution 48 | 49 | The problem with position control of scara using ros_control is that, when load the controller from a launch file, it always looks for robot model at "/robot_description", instead of what I defined "/scara_robot_left/robot_description" in the parameter server. I can change name to"/robot_description", but there is no good way to control multiple different robots and having their models under same name in parameter. Some posts about this problem: 50 | 51 | [gazebo ros control tutorial](http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros) 52 | 53 | [adding robot_description to parameter server](http://answers.ros.org/question/61479/adding-robot_description-to-parameter-server/) 54 | 55 | [Running controller_manager spawner with /mybot/robot_description?](http://answers.ros.org/question/198929/running-controller_manager-spawner-with-mybotrobot_description/) 56 | 57 | [gazebo_ros_control: Use the model NodeHandle to get the robot_description parameter](https://github.com/ros-simulation/gazebo_ros_pkgs/pull/134) 58 | 59 | [Using gazebo_ros_control with SDF instead of URDF](http://answers.ros.org/question/223196/using-gazebo_ros_control-with-sdf-instead-of-urdf/) 60 | 61 | [gazebo_ros_control: plugin doesn't look for the robot model in its own namespace](https://github.com/ros-simulation/gazebo_ros_pkgs/issues/112) 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/scara_upper_boundary_maintainer.cpp: -------------------------------------------------------------------------------- 1 | // a node that publish on following two topics 2 | // "/scara_left_upper_boundary" 3 | // "/scara_left_upper_boundary" 4 | // these two topics describe the upper boundary in their planned motion 5 | // used as state machines for the purpose of collision avoidance 6 | 7 | // ros communication: 8 | // keep publishing on upper boundary topics 9 | // host two service servers for published data change 10 | // "/scara_left_upper_boundary_change" 11 | // "/scara_right_upper_boundary_change" 12 | 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | // global data 19 | bool g_left_in_action; 20 | double g_left_upper_boundary; 21 | bool g_right_in_action; 22 | double g_right_upper_boundary; 23 | 24 | // callback to update global upper boundary data for left scara 25 | bool leftChangeCallback(two_scara_collaboration::upper_boundary_changeRequest& request 26 | , two_scara_collaboration::upper_boundary_changeResponse& response) { 27 | if (request.in_action_change) { 28 | g_left_in_action = request.in_action; 29 | } 30 | if (request.upper_boundary_change) { 31 | g_left_upper_boundary = request.upper_boundary; 32 | } 33 | response.change_is_done = true; 34 | return response.change_is_done; 35 | } 36 | 37 | // callback to update global upper boundary data for right scara 38 | bool rightChangeCallback(two_scara_collaboration::upper_boundary_changeRequest& request 39 | , two_scara_collaboration::upper_boundary_changeResponse& response) { 40 | if (request.in_action_change) { 41 | g_right_in_action = request.in_action; 42 | } 43 | if (request.upper_boundary_change) { 44 | g_right_upper_boundary = request.upper_boundary; 45 | } 46 | response.change_is_done = true; 47 | return response.change_is_done; 48 | } 49 | 50 | 51 | int main(int argc, char** argv) { 52 | ros::init(argc, argv, "scara_upper_boundary_maintainer"); 53 | ros::NodeHandle nh; 54 | 55 | // initialize global data 56 | g_left_in_action = false; 57 | g_left_upper_boundary = 0.6; 58 | g_right_in_action = false; 59 | g_right_upper_boundary = -0.6; 60 | 61 | // initialize publisher on topic "/scara_left_upper_boundary" 62 | ros::Publisher scara_left_upper_boundary_publisher 63 | = nh.advertise("/scara_left_upper_boundary", 1); 64 | two_scara_collaboration::scara_upper_boundary scara_left_upper_boundary_msg; 65 | // initialize publisher on topic "/scara_left_upper_boundary" 66 | ros::Publisher scara_right_upper_boundary_publisher 67 | = nh.advertise("/scara_right_upper_boundary", 1); 68 | two_scara_collaboration::scara_upper_boundary scara_right_upper_boundary_msg; 69 | // initialize service server of "/scara_left_upper_boundary_change" 70 | ros::ServiceServer left_change_server 71 | = nh.advertiseService("/scara_left_upper_boundary_change", leftChangeCallback); 72 | // initialize service server of "/scara_right_upper_boundary_change" 73 | ros::ServiceServer right_change_server 74 | = nh.advertiseService("/scara_right_upper_boundary_change", rightChangeCallback); 75 | 76 | // publish loop 77 | ros::Rate rate_timer(50); 78 | while (ros::ok()) { 79 | // prepare published msg 80 | scara_left_upper_boundary_msg.in_action = g_left_in_action; 81 | scara_left_upper_boundary_msg.upper_boundary = g_left_upper_boundary; 82 | scara_right_upper_boundary_msg.in_action = g_right_in_action; 83 | scara_right_upper_boundary_msg.upper_boundary = g_right_upper_boundary; 84 | // publish the messages 85 | scara_left_upper_boundary_publisher.publish(scara_left_upper_boundary_msg); 86 | scara_right_upper_boundary_publisher.publish(scara_right_upper_boundary_msg); 87 | 88 | rate_timer.sleep(); 89 | ros::spinOnce(); 90 | } 91 | } 92 | 93 | 94 | -------------------------------------------------------------------------------- /src/cylinder_active_pool.cpp: -------------------------------------------------------------------------------- 1 | // this node maintain a pool of reachable blocks 2 | // reachable means in the range of the the scara robots 3 | // and could be fetched before it leaves robot's range 4 | // this cylinder active pool has follow read/write property: 5 | // published for public read access 6 | // write by sending a service request 7 | 8 | // add into the pool only when it goes into the reachable range 9 | // delete from the pool 10 | // when it goes out the range 11 | // when it is claimed by one of the robot motion planner 12 | 13 | // ros communication: 14 | // subscribe to topic "/cylinder_blocks_poses" 15 | // publish the topic "/cylinder_active_pool" 16 | // host a service for pool change "/cylinder_pool_claim" 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // the upper and lower limit of reachable range along the direction of conveyor 26 | const double RANGE_UPPER_LIMIT = sqrt(pow(1.8, 2) - pow(1.5, 2)); 27 | const double RANGE_LOWER_LIMIT = -sqrt(pow(1.8, 2) - pow(1.5, 2)) + 0.5; 28 | 29 | // global variables 30 | std::vector g_cylinder_active_pool; // the POOL, elements are index of cylinders 31 | // add cylinder to pool from this index when checking x position 32 | // avoid repeated adding, some cylinders may be grasped by scara and back in range 33 | int cylinder_add_start_index = 0; 34 | 35 | 36 | // add and delete from the pool when in or out of the range 37 | void cylinderPosesCallback(const two_scara_collaboration::cylinder_blocks_poses& cylinder_poses_msg) { 38 | // get the x coordinates 39 | std::vector current_cylinder_x; 40 | int cylinder_quantity = cylinder_poses_msg.x.size(); 41 | current_cylinder_x.resize(cylinder_quantity); 42 | current_cylinder_x = cylinder_poses_msg.x; 43 | 44 | // check for pool addition 45 | // add cylinders only after the cylinder_start_index 46 | for (int i=cylinder_add_start_index; i RANGE_LOWER_LIMIT 48 | && current_cylinder_x[i] < RANGE_UPPER_LIMIT) { 49 | bool in_pool = false; // presume not in the pool 50 | for (int j=0; j("/cylinder_active_pool", 1); 114 | std_msgs::Int8MultiArray cylinder_active_pool_msg; 115 | // initialize service server of "/cylinder_pool_claim" 116 | ros::ServiceServer cylinder_pool_server 117 | = nh.advertiseService("/cylinder_pool_claim", cylinderPoolCallback); 118 | 119 | // publish loop for the pool 120 | // inside is publish frequency, should be lower than "/cylinder_blocks_poses" 121 | // "/cylinder_blocks_poses" is measured at about 200hz 122 | ros::Rate rate_timer(100); 123 | while (ros::ok()) { 124 | cylinder_active_pool_msg.data.clear(); 125 | cylinder_active_pool_msg.data = g_cylinder_active_pool; 126 | cylinder_pool_publisher.publish(cylinder_active_pool_msg); 127 | 128 | rate_timer.sleep(); // slow down publish rate 129 | ros::spinOnce(); 130 | } 131 | 132 | } 133 | 134 | -------------------------------------------------------------------------------- /src/cylinder_blocks_poses_publisher.cpp: -------------------------------------------------------------------------------- 1 | // this node will publish the topic "cylinder_blocks_poses" 2 | // including all current cylinder blocks, pose is 3-D position 3 | 4 | // ros communication: 5 | // subscribe to topic "/current_cylinder_blocks" 6 | // subscribe to topic "/gazebo/model_states" 7 | // publish the topic "/cylinder_blocks_poses" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | // global variables 17 | int g_cylinder_quantity; 18 | std::vector g_current_cylinder_blocks; 19 | std::vector g_x; 20 | std::vector g_y; 21 | std::vector g_z; 22 | bool g_current_cylinder_callback_started = false; 23 | bool g_cylinder_poses_updated = false; // act as frequency control of publish loop 24 | 25 | std::string intToString(int a) { 26 | std::stringstream ss; 27 | ss << a; 28 | return ss.str(); 29 | } 30 | 31 | void currentCylinderCallback(const std_msgs::Int8MultiArray& current_cylinder_blocks) { 32 | // this topic contains information of what cylinder blocks have been spawned 33 | if (!g_current_cylinder_callback_started) { 34 | // set first time started flag to true 35 | g_current_cylinder_callback_started = true; 36 | ROS_INFO("current cylinder callback has been invoked first time"); 37 | } 38 | g_cylinder_quantity = current_cylinder_blocks.data.size(); 39 | g_current_cylinder_blocks.resize(g_cylinder_quantity); 40 | g_current_cylinder_blocks = current_cylinder_blocks.data; 41 | } 42 | 43 | void modelStatesCallback(const gazebo_msgs::ModelStates& current_model_states) { 44 | // this callback update global values of cylinder positions 45 | if (g_current_cylinder_callback_started) { 46 | // only update when currentCylinderCallback has been invoked the first time 47 | // get cylinder blocks positions according to settings in g_current_cylinder_blocks 48 | std::vector cylinder_x; 49 | std::vector cylinder_y; 50 | std::vector cylinder_z; 51 | cylinder_x.resize(g_cylinder_quantity); 52 | cylinder_y.resize(g_cylinder_quantity); 53 | cylinder_z.resize(g_cylinder_quantity); 54 | // find position of all current cylinders in topic message 55 | bool cylinder_poses_completed = true; 56 | for (int i=0; i("cylinder_blocks_poses", 1); 114 | two_scara_collaboration::cylinder_blocks_poses current_poses_msg; 115 | 116 | // publishing loop 117 | while (ros::ok()) { 118 | if (g_cylinder_poses_updated) { 119 | // only publish when cylinder positions are updated 120 | // no need to publish repeated data 121 | g_cylinder_poses_updated = false; // set flag to false 122 | // there is tiny possibility that g_x is not in the length of g_cylinder_quantity 123 | int local_cylinder_quantity = g_x.size(); // so get length of g_x 124 | current_poses_msg.x.resize(local_cylinder_quantity); 125 | current_poses_msg.y.resize(local_cylinder_quantity); 126 | current_poses_msg.z.resize(local_cylinder_quantity); 127 | current_poses_msg.x = g_x; 128 | current_poses_msg.y = g_y; 129 | current_poses_msg.z = g_z; 130 | cylinder_poses_publisher.publish(current_poses_msg); 131 | } 132 | ros::spinOnce(); 133 | } 134 | return 0; 135 | } 136 | 137 | 138 | 139 | -------------------------------------------------------------------------------- /src/cylinder_blocks_spawner.cpp: -------------------------------------------------------------------------------- 1 | // spawn the red and blue cylinders on the conveyor belt 2 | // and give them initial speed (by apply_body_wrench) to slide on conveyor 3 | 4 | // ros communications: 5 | // spawn model through gazebo service: /gazebo/spawn_urdf_model 6 | // initialize cylinder speed: /gazebo/apply_body_wrench 7 | // get urdf file path of cylinder blocks from parameter server 8 | // publish all current blocks through topic: /current_cylinder_blocks 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // int to string converter 20 | std::string intToString(int a) { 21 | std::stringstream ss; 22 | ss << a; 23 | return ss.str(); 24 | } 25 | 26 | int main(int argc, char **argv) { 27 | ros::init(argc, argv, "cylinder_blocks_spawner"); 28 | ros::NodeHandle nh; 29 | 30 | // service client for service /gazebo/spawn_urdf_model 31 | ros::ServiceClient spawn_model_client 32 | = nh.serviceClient("/gazebo/spawn_urdf_model"); 33 | gazebo_msgs::SpawnModel spawn_model_srv_msg; // service message 34 | // service client for service /gazebo/apply_body_wrench 35 | ros::ServiceClient apply_wrench_client 36 | = nh.serviceClient("/gazebo/apply_body_wrench"); 37 | gazebo_msgs::ApplyBodyWrench apply_wrench_srv_msg; // service message 38 | 39 | // publisher for /current_cylinder_blocks 40 | ros::Publisher current_cylinder_publisher 41 | = nh.advertise("current_cylinder_blocks", 1); 42 | std_msgs::Int8MultiArray current_cylinder_msg; 43 | current_cylinder_msg.data.clear(); 44 | 45 | // make sure /gazebo/spawn_urdf_model service is ready 46 | bool service_ready = false; 47 | while (!service_ready) { 48 | service_ready = ros::service::exists("/gazebo/spawn_urdf_model", true); 49 | ROS_INFO("waiting for spawn_urdf_model service"); 50 | ros::Duration(0.5).sleep(); 51 | } 52 | ROS_INFO("spawn_urdf_model service is ready"); 53 | 54 | // make sure /gazebo/apply_body_wrench service is ready 55 | service_ready = false; 56 | while (!service_ready) { 57 | service_ready = ros::service::exists("/gazebo/apply_body_wrench", true); 58 | ROS_INFO("waiting for apply_body_wrench service"); 59 | ros::Duration(0.5).sleep(); 60 | } 61 | ROS_INFO("apply_body_wrench service is ready"); 62 | 63 | // get file path of cylinder blocks from parameter server 64 | std::string red_cylinder_path; 65 | std::string blue_cylinder_path; 66 | bool get_red_path, get_blue_path; 67 | get_red_path = nh.getParam("/red_cylinder_path", red_cylinder_path); 68 | get_blue_path = nh.getParam("/blue_cylinder_path", blue_cylinder_path); 69 | if (!(get_red_path && get_blue_path)) 70 | return 0; // return if fail to get parameters 71 | 72 | // prepare the xml for service call, read urdf into string 73 | std::ifstream red_inXml, blue_inXml; 74 | std::stringstream red_strStream, blue_strStream; 75 | std::string red_xmlStr, blue_xmlStr; 76 | // red cylinder 77 | red_inXml.open(red_cylinder_path.c_str()); 78 | red_strStream << red_inXml.rdbuf(); 79 | red_xmlStr = red_strStream.str(); 80 | // blue cylinder 81 | blue_inXml.open(blue_cylinder_path.c_str()); 82 | blue_strStream << blue_inXml.rdbuf(); 83 | blue_xmlStr = blue_strStream.str(); 84 | 85 | // prepare the spawn model service message 86 | spawn_model_srv_msg.request.initial_pose.position.x = 4.0; 87 | spawn_model_srv_msg.request.initial_pose.position.z = 0.2; // on the conveyor belt 88 | spawn_model_srv_msg.request.initial_pose.orientation.x = 0.0; 89 | spawn_model_srv_msg.request.initial_pose.orientation.y = 0.0; 90 | spawn_model_srv_msg.request.initial_pose.orientation.z = 0.0; 91 | spawn_model_srv_msg.request.initial_pose.orientation.w = 1.0; 92 | spawn_model_srv_msg.request.reference_frame = "world"; 93 | // reference_frame has to be empty, map or world. Otherwise gazebo will return error 94 | // http://answers.ros.org/question/65077/errors-while-applying-force-on-a-model/ 95 | 96 | // prepare the apply body wrench service message 97 | // the exerted force and duration will decide the initial speed 98 | ros::Time time_temp(0, 0); 99 | ros::Duration duration_temp(0, 1000000); 100 | apply_wrench_srv_msg.request.wrench.force.x = -0.0012; 101 | apply_wrench_srv_msg.request.wrench.force.y = 0.0; 102 | apply_wrench_srv_msg.request.wrench.force.z = 0.0; 103 | apply_wrench_srv_msg.request.start_time = time_temp; 104 | apply_wrench_srv_msg.request.duration = duration_temp; 105 | apply_wrench_srv_msg.request.reference_frame = "world"; 106 | 107 | // begin spawn cylinder blocks and give an initial speed on conveyor 108 | int i = 0; // index the cylinder blocks 109 | while (ros::ok()) { 110 | std::string index_string = intToString(i); 111 | std::string model_name; 112 | int8_t color_index; // 0 is red, 1 is blue 113 | 114 | // prepare spawn model service message 115 | spawn_model_srv_msg.request.initial_pose.position.y 116 | = (float)rand()/(float)(RAND_MAX) * 0.8 - 0.4; // random between -0.4 to 0.4 117 | ROS_INFO_STREAM("y position of new cylinder: " 118 | << spawn_model_srv_msg.request.initial_pose.position.y); 119 | if ((rand() - RAND_MAX/2) > 0) { 120 | // then choose red cylinder 121 | color_index = 0; 122 | model_name = "red_cylinder_" + index_string; // initialize model_name 123 | spawn_model_srv_msg.request.model_name = model_name; 124 | spawn_model_srv_msg.request.robot_namespace = "red_cylinder_" + index_string; 125 | spawn_model_srv_msg.request.model_xml = red_xmlStr; 126 | } 127 | else { 128 | // then choose blue cylinder 129 | color_index = 1; 130 | model_name = "blue_cylinder_" + index_string; 131 | spawn_model_srv_msg.request.model_name = model_name; 132 | spawn_model_srv_msg.request.robot_namespace = "blue_cylinder_" + index_string; 133 | spawn_model_srv_msg.request.model_xml = blue_xmlStr; 134 | } 135 | // call spawn model service 136 | bool call_service = spawn_model_client.call(spawn_model_srv_msg); 137 | if (call_service) { 138 | if (spawn_model_srv_msg.response.success) { 139 | ROS_INFO_STREAM(model_name << " has been spawned"); 140 | } 141 | else { 142 | ROS_INFO_STREAM(model_name << " spawn failed"); 143 | } 144 | } 145 | else { 146 | ROS_INFO("fail in first call"); 147 | ROS_ERROR("fail to connect with gazebo server"); 148 | return 0; 149 | } 150 | 151 | // prepare apply body wrench service message 152 | apply_wrench_srv_msg.request.body_name = model_name + "::base_link"; 153 | // call apply body wrench service 154 | call_service = apply_wrench_client.call(apply_wrench_srv_msg); 155 | if (call_service) { 156 | if (apply_wrench_srv_msg.response.success) { 157 | ROS_INFO_STREAM(model_name << " speed initialized"); 158 | } 159 | else { 160 | ROS_INFO_STREAM(model_name << " fail to initialize speed"); 161 | } 162 | } 163 | else { 164 | ROS_ERROR("fail to connect with gazebo server"); 165 | return 0; 166 | } 167 | 168 | // publish current cylinder blocks status, all cylinder blocks will be published 169 | // no matter if it's successfully spawned, or successfully initialized in speed 170 | current_cylinder_msg.data.push_back(color_index); 171 | current_cylinder_publisher.publish(current_cylinder_msg); 172 | 173 | // loop end, increase index by 1, add blank line 174 | i = i + 1; 175 | ROS_INFO_STREAM(""); 176 | 177 | ros::spinOnce(); 178 | ros::Duration(7.0).sleep(); // frequency control, spawn one cylinder in each loop 179 | // delay time decides density of the cylinders 180 | } 181 | return 0; 182 | } 183 | 184 | 185 | 186 | -------------------------------------------------------------------------------- /urdf/scara_robot_right.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 | 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 | 75 | 76 | 77 | 78 | 79 | Gazebo/Grey 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 | 116 | 117 | 118 | 119 | 120 | Gazebo/Grey 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 | 146 | 147 | 148 | 149 | 150 | 151 | 155 | 156 | 157 | 158 | 159 | Gazebo/White 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 196 | 197 | 198 | 199 | 200 | Gazebo/White 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 234 | 235 | 236 | 237 | 238 | Gazebo/White 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 272 | 273 | 274 | 275 | 276 | Gazebo/White 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 310 | 311 | 312 | 313 | 314 | Gazebo/White 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | -------------------------------------------------------------------------------- /src/scara_joint_controller.cpp: -------------------------------------------------------------------------------- 1 | // the joint controller for all four joint of two scara robots 2 | // a PD controller read joint angle & rate, and apply joint torque 3 | 4 | // the "Joint" class is copied from my previous project, with some minor changes 5 | // one instance of this class to control one joint of the robots 6 | // ros communication of the class: 7 | // service clients, /gazebo/get_joint_properties, /gazebo/apply_joint_effort 8 | // publish joint state, including posiiton, velocity, torque 9 | // subscribe to position command 10 | // service server for kp & kv tuning 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | // class definition 22 | class Joint { 23 | public: 24 | Joint(ros::NodeHandle nh, std::string joint_name, double pos_cmd, double dt); // constructor 25 | ~Joint() {}; // destructor 26 | void getJointState(); 27 | void jointTrqControl(); 28 | void kpkvSetting(double kp, double kv); 29 | private: 30 | // callback for the pos_cmd subscriber 31 | void posCmdCB(const std_msgs::Float64& pos_cmd_msg); 32 | // callback for kpkv service server 33 | bool kpkvCallback(two_scara_collaboration::kpkv_msgRequest& request 34 | , two_scara_collaboration::kpkv_msgResponse& response); 35 | // service clients 36 | ros::ServiceClient get_jnt_state_client; 37 | ros::ServiceClient set_trq_client; 38 | // publisher objects 39 | ros::Publisher trq_publisher; 40 | ros::Publisher vel_publisher; 41 | ros::Publisher pos_publisher; 42 | ros::Publisher joint_state_publisher; 43 | // subscriber object 44 | ros::Subscriber pos_cmd_subscriber; 45 | // gazebo/sensor messages 46 | gazebo_msgs::GetJointProperties get_joint_state_srv_msg; 47 | gazebo_msgs::ApplyJointEffort effort_cmd_srv_msg; 48 | sensor_msgs::JointState joint_state_msg; 49 | // position/velocity/torque messages to be published 50 | std_msgs::Float64 pos_msg; // position 51 | std_msgs::Float64 vel_msg; // velocity 52 | std_msgs::Float64 trq_msg; // torque 53 | // kpkv service server 54 | ros::ServiceServer kpkv_server; 55 | 56 | // control parameters 57 | double pos_cur; // current joint position 58 | double vel_cur; // current joint velocity 59 | double pos_cmd; // joint position from commander 60 | double pos_err; // error between pos_cmd and pos_cur 61 | double trq_cmd; // torque to be published 62 | double kp; 63 | double kv; 64 | // other parameters 65 | std::string joint_name; 66 | }; 67 | 68 | Joint::Joint(ros::NodeHandle nh, std::string joint_name, double pos_cmd, double dt) { 69 | // initialize parameters 70 | this -> joint_name = joint_name; 71 | this -> pos_cmd = pos_cmd; // give a initial position 72 | ros::Duration duration(dt); 73 | kp = 10.0; 74 | kv = 3.0; 75 | 76 | pos_cur = 0.0; 77 | 78 | // initialize gazebo clients 79 | get_jnt_state_client = nh.serviceClient("/gazebo/get_joint_properties"); 80 | set_trq_client = nh.serviceClient("/gazebo/apply_joint_effort"); 81 | // initialize publisher objects 82 | // check if there is "::" symbol in the string, replace with "_" 83 | std::string joint_name_published = joint_name; 84 | std::size_t found = joint_name_published.find("::"); 85 | if (found!=std::string::npos) { 86 | joint_name_published.replace(found, 2, "_"); 87 | } 88 | pos_publisher = nh.advertise(joint_name_published + "_pos", 1); 89 | vel_publisher = nh.advertise(joint_name_published + "_vel", 1); 90 | trq_publisher = nh.advertise(joint_name_published + "_trq", 1); 91 | joint_state_publisher = nh.advertise(joint_name_published + "_states", 1); 92 | // initialize subscriber object 93 | pos_cmd_subscriber = nh.subscribe(joint_name_published + "_pos_cmd", 1, &Joint::posCmdCB, this); 94 | // initialize kpkv service server 95 | kpkv_server = nh.advertiseService(joint_name_published + "_kpkv_service", &Joint::kpkvCallback, this); 96 | 97 | // set up get_joint_state_srv_msg 98 | get_joint_state_srv_msg.request.joint_name = joint_name; 99 | // set up effort_cmd_srv_msg 100 | effort_cmd_srv_msg.request.joint_name = joint_name; 101 | effort_cmd_srv_msg.request.effort = 0.0; 102 | effort_cmd_srv_msg.request.duration = duration; 103 | // set up joint_state_msg 104 | joint_state_msg.header.stamp = ros::Time::now(); 105 | joint_state_msg.name.push_back(joint_name); 106 | joint_state_msg.position.push_back(0.0); 107 | joint_state_msg.velocity.push_back(0.0); 108 | } 109 | 110 | void Joint::posCmdCB(const std_msgs::Float64& pos_cmd_msg) { 111 | // too much information 112 | // ROS_INFO("received value of %s_pos_cmd is: %f", joint_name.c_str(), pos_cmd_msg.data); 113 | pos_cmd = pos_cmd_msg.data; 114 | } 115 | 116 | void Joint::getJointState() { 117 | // get joint state 118 | get_jnt_state_client.call(get_joint_state_srv_msg); 119 | // publish joint position 120 | pos_cur = get_joint_state_srv_msg.response.position[0]; 121 | pos_msg.data = pos_cur; 122 | pos_publisher.publish(pos_msg); 123 | // publish joint velocity 124 | vel_cur = get_joint_state_srv_msg.response.rate[0]; 125 | vel_msg.data = vel_cur; 126 | vel_publisher.publish(vel_msg); 127 | // publish joint_state_msg 128 | joint_state_msg.header.stamp = ros::Time::now(); 129 | joint_state_msg.position[0] = pos_cur; 130 | joint_state_msg.velocity[0] = vel_cur; 131 | joint_state_publisher.publish(joint_state_msg); 132 | } 133 | 134 | // calculate joint torque, publish them, send to gazebo 135 | void Joint::jointTrqControl() { 136 | pos_err = pos_cmd - pos_cur; 137 | // watch for periodicity 138 | // if (pos_err > M_PI) 139 | // pos_err = pos_err - 2 * M_PI; 140 | // if (pos_err > M_PI) 141 | // pos_err = pos_err + 2 * M_PI; 142 | // control algorithm in one line 143 | trq_cmd = kp * pos_err - kv * vel_cur; 144 | // publish the torque message 145 | trq_msg.data = trq_cmd; 146 | trq_publisher.publish(trq_msg); 147 | // send torque command to gazebo 148 | effort_cmd_srv_msg.request.effort = trq_cmd; 149 | set_trq_client.call(effort_cmd_srv_msg); 150 | // make sure service call was successful 151 | bool result = effort_cmd_srv_msg.response.success; 152 | if (!result) 153 | ROS_WARN("service call to apply_joint_effort failed!"); 154 | } 155 | 156 | void Joint::kpkvSetting(double kp, double kv) { 157 | this -> kp = kp; 158 | this -> kv = kv; 159 | } 160 | 161 | bool Joint::kpkvCallback(two_scara_collaboration::kpkv_msgRequest& request 162 | , two_scara_collaboration::kpkv_msgResponse& response) { 163 | ROS_INFO("kpkvCallback activated"); 164 | kp = request.kp; 165 | kv = request.kv; 166 | // joint_name has to be converted to c_str() for ROS_INFO output 167 | ROS_INFO("%s: kp has been set to %f, kv has been set to %f", joint_name.c_str(), kp, kv); 168 | response.setting_is_done = true; 169 | return true; 170 | } 171 | 172 | 173 | int main(int argc, char **argv) { 174 | ros::init(argc, argv, "scara_joint_controller"); 175 | ros::NodeHandle nh; 176 | ros::Duration half_sec(0.5); 177 | 178 | // make sure apply_joint_effort service is ready 179 | bool service_ready = false; 180 | while (!service_ready) { 181 | service_ready = ros::service::exists("/gazebo/apply_joint_effort",true); 182 | ROS_INFO("waiting for apply_joint_effort service"); 183 | half_sec.sleep(); 184 | } 185 | ROS_INFO("apply_joint_effort service exists"); 186 | // make sure get_joint_state_client service is ready 187 | service_ready = false; 188 | while (!service_ready) { 189 | service_ready = ros::service::exists("/gazebo/get_joint_properties",true); 190 | ROS_INFO("waiting for /gazebo/get_joint_properties service"); 191 | half_sec.sleep(); 192 | } 193 | ROS_INFO("/gazebo/get_joint_properties service exists"); 194 | 195 | double dt = 0.01; // sample time for the joint controller 196 | 197 | // instantiate 4 joint instances 198 | Joint scara_left_joint1(nh, "scara_robot_left::rotation1", -0.78, dt); 199 | Joint scara_left_joint2(nh, "scara_robot_left::rotation2", 2.1, dt); 200 | Joint scara_right_joint1(nh, "scara_robot_right::rotation1", -0.78, dt); 201 | Joint scara_right_joint2(nh, "scara_robot_right::rotation2", 2.1, dt); 202 | 203 | // set kp & kv here after being tuned 204 | scara_left_joint1.kpkvSetting(80, 16); 205 | scara_left_joint2.kpkvSetting(1, 0.2); 206 | scara_right_joint1.kpkvSetting(80, 16); 207 | scara_right_joint2.kpkvSetting(1, 0.2); 208 | 209 | ros::Rate rate_timer(1/dt); 210 | while (ros::ok()) { 211 | // get joint state(pos, vel) and publish them 212 | scara_left_joint1.getJointState(); 213 | scara_left_joint2.getJointState(); 214 | scara_right_joint1.getJointState(); 215 | scara_right_joint2.getJointState(); 216 | 217 | // calculate the torque for each joitn and publish them 218 | scara_left_joint1.jointTrqControl(); 219 | scara_left_joint2.jointTrqControl(); 220 | scara_right_joint1.jointTrqControl(); 221 | scara_right_joint2.jointTrqControl(); 222 | 223 | ros::spinOnce(); // update pos_cmd, kpkv 224 | rate_timer.sleep(); // sleep for the sample time 225 | } 226 | 227 | } 228 | 229 | 230 | 231 | 232 | -------------------------------------------------------------------------------- /urdf/scara_robot_left.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 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 | 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 | 86 | 87 | 88 | 89 | 90 | Gazebo/Grey 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 | 126 | 127 | 128 | 129 | 130 | Gazebo/Grey 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 166 | 167 | 168 | 169 | 170 | Gazebo/White 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 208 | 209 | 210 | 211 | 212 | Gazebo/White 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 246 | 247 | 248 | 249 | 250 | Gazebo/White 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 284 | 285 | 286 | 287 | 288 | Gazebo/White 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 322 | 323 | 324 | 325 | 326 | Gazebo/White 327 | 328 | 329 | 330 | 331 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | -------------------------------------------------------------------------------- /src/scara_gripper_action_server.cpp: -------------------------------------------------------------------------------- 1 | // the action server for the gripper control 2 | // except receiving command from client, it keeps the gripper in its last status 3 | 4 | // ros communication: 5 | // host an action server "gripper_action" to receive command on the gripper 6 | // service client for gazebo service "/gazebo/apply_joint_effort" 7 | 8 | // action server is not really necessary here 9 | // because the server only receives commanded status of the gripper 10 | // no feedback or result are need, a service server will also do fine 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // constants for joint effort and duration, values tuned 18 | const double GRIPPER_UP_EFFORT = 0.001; 19 | const double GRIPPER_DOWN_EFFORT = 0.0; 20 | const double GRIPPER_GRASP_EFFORT_ABS = 0.002; // absolute value 21 | const double GRIPPER_RELEASE_EFFORT_ABS = 0.0001; // absolute value 22 | const ros::Duration EFFORT_DURATION(0, 250000000); // 0.25s, longer than loop time of 0.1s 23 | const double EFFORT_FREQUENCY = 1/0.1; // this decide the reaction speed of action server 24 | 25 | // global variables acted as state machine, to keep gripper status of last command 26 | bool g_up_down_left = false; // down is true, means stretch out gripper for task 27 | bool g_up_down_right = false; 28 | bool g_grasp_release_left = false; // grasp is true 29 | bool g_grasp_release_right = false; 30 | 31 | // action server class definition 32 | class GripperActionServer { 33 | public: 34 | GripperActionServer(); // constructor 35 | ~GripperActionServer(void) {}; // destructor 36 | // action callback 37 | void executeCb(const 38 | actionlib::SimpleActionServer::GoalConstPtr& goal); 39 | 40 | // service client for "/gazebo/apply_joint_effort" 41 | // put service client in public so that it can be accessed from outside 42 | ros::ServiceClient joint_effort_client_; 43 | // service messages 44 | gazebo_msgs::ApplyJointEffort joint_effort_left0_srv_msg_; // left0 indicate gripper joint 45 | gazebo_msgs::ApplyJointEffort joint_effort_left1_srv_msg_; // left(i) indicate ith finger 46 | gazebo_msgs::ApplyJointEffort joint_effort_left2_srv_msg_; 47 | gazebo_msgs::ApplyJointEffort joint_effort_left3_srv_msg_; 48 | gazebo_msgs::ApplyJointEffort joint_effort_left4_srv_msg_; 49 | gazebo_msgs::ApplyJointEffort joint_effort_right0_srv_msg_; // right0 indicate gripper joint 50 | gazebo_msgs::ApplyJointEffort joint_effort_right1_srv_msg_; // right(i) indicate ith finger 51 | gazebo_msgs::ApplyJointEffort joint_effort_right2_srv_msg_; 52 | gazebo_msgs::ApplyJointEffort joint_effort_right3_srv_msg_; 53 | gazebo_msgs::ApplyJointEffort joint_effort_right4_srv_msg_; 54 | private: 55 | ros::NodeHandle nh_; 56 | // create a "SimpleActionServer" call "as_" 57 | actionlib::SimpleActionServer as_; 58 | 59 | two_scara_collaboration::scara_gripperGoal goal_; // goal message 60 | two_scara_collaboration::scara_gripperResult result_; // result message 61 | two_scara_collaboration::scara_gripperFeedback feedback_; // feedback message 62 | }; 63 | 64 | // constructor 65 | GripperActionServer::GripperActionServer(): 66 | as_(nh_, "gripper_action", boost::bind(&GripperActionServer::executeCb, this, _1), false) 67 | { 68 | ROS_INFO("in constructor of GripperActionServer..."); 69 | 70 | // initialize the apply joint effort service client 71 | joint_effort_client_ 72 | = nh_.serviceClient("/gazebo/apply_joint_effort"); 73 | 74 | // check if service "/gazebo/apply_joint_effort" is ready 75 | bool service_ready = false; 76 | while (!service_ready) { 77 | service_ready = ros::service::exists("/gazebo/apply_joint_effort", true); 78 | ROS_INFO("waiting for apply_joint_effort service"); 79 | ros::Duration(0.5).sleep(); 80 | } 81 | ROS_INFO("apply_joint_effort service is ready"); 82 | 83 | // preparation for some service message 84 | joint_effort_left0_srv_msg_.request.joint_name = "scara_robot_left::gripper_joint"; // left 85 | joint_effort_left1_srv_msg_.request.joint_name = "scara_robot_left::finger1_joint"; 86 | joint_effort_left2_srv_msg_.request.joint_name = "scara_robot_left::finger2_joint"; 87 | joint_effort_left3_srv_msg_.request.joint_name = "scara_robot_left::finger3_joint"; 88 | joint_effort_left4_srv_msg_.request.joint_name = "scara_robot_left::finger4_joint"; 89 | joint_effort_right0_srv_msg_.request.joint_name = "scara_robot_right::gripper_joint"; // right 90 | joint_effort_right1_srv_msg_.request.joint_name = "scara_robot_right::finger1_joint"; 91 | joint_effort_right2_srv_msg_.request.joint_name = "scara_robot_right::finger2_joint"; 92 | joint_effort_right3_srv_msg_.request.joint_name = "scara_robot_right::finger3_joint"; 93 | joint_effort_right4_srv_msg_.request.joint_name = "scara_robot_right::finger4_joint"; 94 | ros::Time time_temp(0, 0); 95 | joint_effort_left0_srv_msg_.request.start_time = time_temp; // set the time to immediately 96 | joint_effort_left1_srv_msg_.request.start_time = time_temp; 97 | joint_effort_left2_srv_msg_.request.start_time = time_temp; 98 | joint_effort_left3_srv_msg_.request.start_time = time_temp; 99 | joint_effort_left4_srv_msg_.request.start_time = time_temp; 100 | joint_effort_right0_srv_msg_.request.start_time = time_temp; 101 | joint_effort_right1_srv_msg_.request.start_time = time_temp; 102 | joint_effort_right2_srv_msg_.request.start_time = time_temp; 103 | joint_effort_right3_srv_msg_.request.start_time = time_temp; 104 | joint_effort_right4_srv_msg_.request.start_time = time_temp; 105 | joint_effort_left0_srv_msg_.request.duration = EFFORT_DURATION; // set the duration 106 | joint_effort_left1_srv_msg_.request.duration = EFFORT_DURATION; 107 | joint_effort_left2_srv_msg_.request.duration = EFFORT_DURATION; 108 | joint_effort_left3_srv_msg_.request.duration = EFFORT_DURATION; 109 | joint_effort_left4_srv_msg_.request.duration = EFFORT_DURATION; 110 | joint_effort_right0_srv_msg_.request.duration = EFFORT_DURATION; 111 | joint_effort_right1_srv_msg_.request.duration = EFFORT_DURATION; 112 | joint_effort_right2_srv_msg_.request.duration = EFFORT_DURATION; 113 | joint_effort_right3_srv_msg_.request.duration = EFFORT_DURATION; 114 | joint_effort_right4_srv_msg_.request.duration = EFFORT_DURATION; 115 | 116 | as_.start(); // start the action server 117 | } 118 | 119 | // callback of the action server 120 | void GripperActionServer::executeCb(const 121 | actionlib::SimpleActionServer::GoalConstPtr& goal) 122 | { 123 | ROS_INFO("in gripper_action callback..."); 124 | ROS_INFO_STREAM("the goal message received:"); 125 | ROS_INFO_STREAM("left gripper setting: " << g_up_down_left << 126 | ", " << g_grasp_release_left); 127 | ROS_INFO_STREAM("right gripper setting: " << g_up_down_right << 128 | ", " << g_grasp_release_right); 129 | 130 | // up down position of left gripper 131 | if (goal->up_down_left == -1) { 132 | g_up_down_left = false; 133 | } 134 | else if (goal->up_down_left == 1) { 135 | g_up_down_left = true; 136 | } // if 0 then change nothing 137 | // up down position of right gripper 138 | if (goal->up_down_right == -1) { 139 | g_up_down_right = false; 140 | } 141 | else if (goal->up_down_right == 1) { 142 | g_up_down_right = true; 143 | } 144 | // grasp release of left gripper 145 | if (goal->grasp_release_left == -1) { 146 | g_grasp_release_left = false; 147 | } 148 | else if (goal->grasp_release_left == 1) { 149 | g_grasp_release_left = true; 150 | } 151 | // grasp release of right gripper 152 | if (goal->grasp_release_right == -1) { 153 | g_grasp_release_right = false; 154 | } 155 | else if (goal->grasp_release_right == 1) { 156 | g_grasp_release_right = true; 157 | } 158 | 159 | result_.result = 1; // the result value doesn't matter 160 | as_.setSucceeded(result_); // tell the client this action is finished 161 | } 162 | 163 | int main(int argc, char **argv) { 164 | ros::init(argc, argv, "scara_gripper_action_server"); 165 | 166 | GripperActionServer as_object; // instance of the class 167 | 168 | // apply all joint effort loop 169 | ros::Rate loop_rate(EFFORT_FREQUENCY); // loop frequency control 170 | ros::Time time_last, time_now; 171 | time_last = ros::Time::now(); 172 | ros::Duration loop_duration; 173 | while (ros::ok()) { 174 | // loop time measurement 175 | time_now = ros::Time::now(); 176 | loop_duration = time_now - time_last; 177 | time_last = time_now; 178 | // ROS_INFO_STREAM("loop duration: " << loop_duration.toSec()); 179 | 180 | // prepare joint effort message 181 | // left gripper 182 | if (g_up_down_left == false) { // gripper goes up 183 | as_object.joint_effort_left0_srv_msg_.request.effort = GRIPPER_UP_EFFORT; 184 | } 185 | else { // gripper goes down 186 | as_object.joint_effort_left0_srv_msg_.request.effort = GRIPPER_DOWN_EFFORT; 187 | } 188 | if (g_grasp_release_left == false) { // left gripper release 189 | as_object.joint_effort_left1_srv_msg_.request.effort = -GRIPPER_RELEASE_EFFORT_ABS; 190 | as_object.joint_effort_left2_srv_msg_.request.effort = GRIPPER_RELEASE_EFFORT_ABS; 191 | as_object.joint_effort_left3_srv_msg_.request.effort = GRIPPER_RELEASE_EFFORT_ABS; 192 | as_object.joint_effort_left4_srv_msg_.request.effort = -GRIPPER_RELEASE_EFFORT_ABS; 193 | } 194 | else { // left gripper grasp 195 | as_object.joint_effort_left1_srv_msg_.request.effort = GRIPPER_GRASP_EFFORT_ABS; 196 | as_object.joint_effort_left2_srv_msg_.request.effort = -GRIPPER_GRASP_EFFORT_ABS; 197 | as_object.joint_effort_left3_srv_msg_.request.effort = -GRIPPER_GRASP_EFFORT_ABS; 198 | as_object.joint_effort_left4_srv_msg_.request.effort = GRIPPER_GRASP_EFFORT_ABS; 199 | } 200 | // right gripper 201 | if (g_up_down_right == false) { // gripper goes up 202 | as_object.joint_effort_right0_srv_msg_.request.effort = GRIPPER_UP_EFFORT; 203 | } 204 | else { // gripper goes down 205 | as_object.joint_effort_right0_srv_msg_.request.effort = GRIPPER_DOWN_EFFORT; 206 | } 207 | if (g_grasp_release_right == false) { // right gripper release 208 | as_object.joint_effort_right1_srv_msg_.request.effort = GRIPPER_RELEASE_EFFORT_ABS; 209 | as_object.joint_effort_right2_srv_msg_.request.effort = -GRIPPER_RELEASE_EFFORT_ABS; 210 | as_object.joint_effort_right3_srv_msg_.request.effort = -GRIPPER_RELEASE_EFFORT_ABS; 211 | as_object.joint_effort_right4_srv_msg_.request.effort = GRIPPER_RELEASE_EFFORT_ABS; 212 | } 213 | else { // right gripper grasp 214 | as_object.joint_effort_right1_srv_msg_.request.effort = -GRIPPER_GRASP_EFFORT_ABS; 215 | as_object.joint_effort_right2_srv_msg_.request.effort = GRIPPER_GRASP_EFFORT_ABS; 216 | as_object.joint_effort_right3_srv_msg_.request.effort = GRIPPER_GRASP_EFFORT_ABS; 217 | as_object.joint_effort_right4_srv_msg_.request.effort = -GRIPPER_GRASP_EFFORT_ABS; 218 | } 219 | 220 | // send service message to gazebo 221 | as_object.joint_effort_client_.call(as_object.joint_effort_left0_srv_msg_); 222 | as_object.joint_effort_client_.call(as_object.joint_effort_left1_srv_msg_); 223 | as_object.joint_effort_client_.call(as_object.joint_effort_left2_srv_msg_); 224 | as_object.joint_effort_client_.call(as_object.joint_effort_left3_srv_msg_); 225 | as_object.joint_effort_client_.call(as_object.joint_effort_left4_srv_msg_); 226 | as_object.joint_effort_client_.call(as_object.joint_effort_right0_srv_msg_); 227 | as_object.joint_effort_client_.call(as_object.joint_effort_right1_srv_msg_); 228 | as_object.joint_effort_client_.call(as_object.joint_effort_right2_srv_msg_); 229 | as_object.joint_effort_client_.call(as_object.joint_effort_right3_srv_msg_); 230 | as_object.joint_effort_client_.call(as_object.joint_effort_right4_srv_msg_); 231 | 232 | loop_rate.sleep(); 233 | ros::spinOnce(); 234 | } 235 | return 0; 236 | } 237 | 238 | 239 | -------------------------------------------------------------------------------- /src/scara_left_motion_planner.cpp: -------------------------------------------------------------------------------- 1 | // test program for scara motion control 2 | // control the scara robot by publishing joint angles on corresponding topic 3 | // one of the two independent motion planner 4 | // all two scara robots are positive configured 5 | 6 | // ros communication: 7 | // subscribe to topic "/current_cylinder_blocks" 8 | // subscribe to topic "/cylinder_blocks_poses" 9 | // subscribe to topic "/scara_robot_left_rotation1_pos" 10 | // subscribe to topic "/scara_robot_left_rotation2_pos" 11 | // publish to topic "/scara_robot_left_rotation1_pos_cmd" 12 | // publish to topic "/scara_robot_left_rotation2_pos_cmd" 13 | // subscribe to topic "/cylinder_active_pool" 14 | // service client to service "/cylinder_pool_claim" 15 | // subscribe to topic "/scara_right_upper_boundary" 16 | // service client to service "/scara_left_upper_boundary_change" 17 | // action client for gripper control, "gripper_action" 18 | 19 | // motion steps decomposed: 20 | // 1.stand by position -> target cylinder position 21 | // 2.target cylinder hovering (for cylinder grasping operation) 22 | // 3.target cylinder position -> drop position 23 | // 4.drop position -> stand by position 24 | // only in motion step 1, the operation may be delayed when the other robot is in operation range 25 | 26 | // upper boundary control method: 27 | // each robot has a state machine contains a boolean and a number 28 | // the boolean indicate if the robot is at upper area of the conveyor 29 | // the number indicate the upper boundary the robot goes 30 | // each robot check the other robot's state and make change to its own 31 | 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | const double CYLINDER_SPEED = 0.12; // the measured absolute speed of the cylinder 46 | const double STEP1_DURANCE = 2.0; // time allocation for step 1 47 | const double STEP2_DURANCE = 4.0; // time allocation for step 2 48 | const double STEP3_DURANCE = 1.0; // time allocation for step 3 49 | const double STEP4_DURANCE = 1.0; // time allocation for step 4 50 | const double MOTION_SAMPLE_TIME = 0.01; 51 | 52 | const double RED_DROP_JOINT1 = 0.3; // drop position for red cylinder 53 | const double RED_DROP_JOINT2 = 2.0; 54 | const double BLUE_DROP_JOINT1 = -1.9; // drop position for blue cylinder 55 | const double BLUE_DROP_JOINT2 = 1.8; 56 | const double STAND_BY_JOINT1 = -0.78; // stand by scara joints 57 | const double STAND_BY_JOINT2 = 2.1; 58 | 59 | const double MAGIC_DISP = 0.03; // real cylinder position compensation 60 | 61 | // global variables 62 | std::vector g_current_cylinders; // 0 or 1 represent red or blue 63 | bool g_current_cylinders_initialized = false; 64 | std::vector g_cylinder_x; // only x y coordinates matter 65 | std::vector g_cylinder_y; 66 | double g_scara_left_r1; // the two joints of scara 67 | double g_scara_left_r2; 68 | std::vector g_cylinder_pool; // the active pool 69 | bool g_right_in_action; // upper boundary status of right scara 70 | double g_right_upper_boundary; 71 | 72 | 73 | // kinematics for the left scara robot 74 | std::vector scara_left_kinematics(std::vector joints) { 75 | // x_generic and y_generic for the generic coordinates used in scara kinematics 76 | double x_generic = cos(joints[0]) + 0.8*cos(joints[0] + joints[1]); 77 | double y_generic = sin(joints[0]) + 0.8*sin(joints[0] + joints[1]); 78 | // convert to coordinates in the view of left scara 79 | std::vector output_position; 80 | output_position.resize(2); 81 | output_position[0] = y_generic; // x coordinate 82 | output_position[1] = 1.5 - x_generic; // y coordinate 83 | return output_position; 84 | } 85 | 86 | // inverse kinematics for the left scara robot 87 | std::vector scara_left_inverse_kinematics(std::vector position) { 88 | // the returned robot configuration is always positive 89 | // convert position to the generic coordinates in scara kinematics 90 | double x_generic = 1.5 - position[1]; 91 | double y_generic = position[0]; 92 | std::vector output_joints; 93 | output_joints.resize(2); 94 | double end_quad_dist = pow(x_generic, 2) + pow(y_generic, 2); 95 | output_joints[0] = atan(y_generic/x_generic) - acos((0.36 + end_quad_dist)/(2*sqrt(end_quad_dist))); 96 | output_joints[1] = M_PI - acos((1.64 - end_quad_dist)/1.6); 97 | // if (abs(output_joints[1]) < 0.2) { 98 | // ROS_INFO_STREAM("IK points: " << x_generic << ", " << y_generic); 99 | // ROS_INFO_STREAM("IK joints: " << output_joints[0] << ", " << output_joints[1]); 100 | // } 101 | // add robust to this inverse kinematics 102 | if ((output_joints[0] != output_joints[0]) || (output_joints[1] != output_joints[1])) { 103 | // there nan in the output, i.e., value inside cos() is out of [-1,1] 104 | output_joints[0] = atan(y_generic/x_generic); 105 | output_joints[1] = 0; 106 | } 107 | return output_joints; 108 | } 109 | 110 | void currentCylinderCallback(const std_msgs::Int8MultiArray& current_cylinder_msg) { 111 | // update current cylinder blocks 112 | if (!g_current_cylinders_initialized) 113 | g_current_cylinders_initialized = true; // set flag to true 114 | g_current_cylinders.resize(current_cylinder_msg.data.size()); 115 | g_current_cylinders = current_cylinder_msg.data; 116 | } 117 | 118 | void cylinderPosesCallback(const two_scara_collaboration::cylinder_blocks_poses& cylinder_poses_msg) { 119 | // update poses message in global variables 120 | int cylinder_quantity = cylinder_poses_msg.x.size(); 121 | g_cylinder_x.resize(cylinder_quantity); 122 | g_cylinder_y.resize(cylinder_quantity); 123 | g_cylinder_x = cylinder_poses_msg.x; 124 | g_cylinder_y = cylinder_poses_msg.y; 125 | } 126 | 127 | void scaraLeftR1PosCallback(const std_msgs::Float64& rotation1_msg) { 128 | // update first joint angle of left scara robot 129 | g_scara_left_r1 = rotation1_msg.data; 130 | } 131 | 132 | void scaraLeftR2PosCallback(const std_msgs::Float64& rotation2_msg) { 133 | // update second joint angle of left scara robot 134 | g_scara_left_r2 = rotation2_msg.data; 135 | } 136 | 137 | void cylinderPoolCallback(const std_msgs::Int8MultiArray& cylinder_pool_msg) { 138 | // update the cylinder active pool 139 | int pool_size = cylinder_pool_msg.data.size(); 140 | g_cylinder_pool.resize(pool_size); 141 | g_cylinder_pool = cylinder_pool_msg.data; 142 | } 143 | 144 | void scaraRightUpperBoundaryCallback(const 145 | two_scara_collaboration::scara_upper_boundary& upper_boundary_msg) { 146 | // update on upper boundary of right scara 147 | g_right_in_action = upper_boundary_msg.in_action; 148 | g_right_upper_boundary = upper_boundary_msg.upper_boundary; 149 | } 150 | 151 | 152 | int main(int argc, char** argv) { 153 | ros::init(argc, argv, "scara_left_motion_planner"); 154 | ros::NodeHandle nh; 155 | 156 | ROS_ERROR("program stamp 1"); 157 | 158 | // initialize subscriber to "/current_cylinder_blocks" 159 | ros::Subscriber current_cylinder_subscriber = nh.subscribe("/current_cylinder_blocks" 160 | , 1, currentCylinderCallback); 161 | // initialize subscriber to "/cylinder_blocks_poses" 162 | ros::Subscriber cylinder_poses_subscriber = nh.subscribe("/cylinder_blocks_poses" 163 | , 1, cylinderPosesCallback); 164 | // initialize subscriber to "/scara_robot_left_rotation1_pos" 165 | ros::Subscriber scara_left_r1_pos_subscriber = nh.subscribe("/scara_robot_left_rotation1_pos" 166 | , 1, scaraLeftR1PosCallback); 167 | // initialize subscriber to "/scara_robot_left_rotation2_pos" 168 | ros::Subscriber scara_left_r2_pos_subscriber = nh.subscribe("/scara_robot_left_rotation2_pos" 169 | , 1, scaraLeftR2PosCallback); 170 | // publish to topic "/scara_robot_left_rotation1_pos_cmd" 171 | ros::Publisher scara_left_r1_cmd_publisher 172 | = nh.advertise("/scara_robot_left_rotation1_pos_cmd", 1); 173 | std_msgs::Float64 scara_left_r1_cmd_msg; 174 | // publish to topic "/scara_robot_left_rotation2_pos_cmd" 175 | ros::Publisher scara_left_r2_cmd_publisher 176 | = nh.advertise("/scara_robot_left_rotation2_pos_cmd", 1); 177 | std_msgs::Float64 scara_left_r2_cmd_msg; 178 | // subscribe to topic "/cylinder_active_pool" 179 | ros::Subscriber cylinder_pool_subscriber = nh.subscribe("/cylinder_active_pool" 180 | , 1, cylinderPoolCallback); 181 | // service client to service "/cylinder_pool_claim" 182 | ros::ServiceClient cylinder_pool_claim_client 183 | = nh.serviceClient("/cylinder_pool_claim"); 184 | two_scara_collaboration::pool_claim_msg cylinder_claim_srv_msg; 185 | // subscribe to topic "/scara_right_upper_boundary" 186 | ros::Subscriber scara_right_upper_boundary_subscriber 187 | = nh.subscribe("/scara_right_upper_boundary", 1, scaraRightUpperBoundaryCallback); 188 | // service client to service "/scara_left_upper_boundary_change" 189 | ros::ServiceClient scara_left_upper_boundary_client 190 | = nh.serviceClient("/scara_left_upper_boundary_change"); 191 | two_scara_collaboration::upper_boundary_change left_boundary_change_srv; 192 | // action client for gripper control, "gripper_action" 193 | actionlib::SimpleActionClient 194 | scara_gripper_action_client("gripper_action", true); 195 | two_scara_collaboration::scara_gripperGoal scara_gripper_goal; 196 | scara_gripper_goal.up_down_right = 0; 197 | scara_gripper_goal.grasp_release_right = 0; 198 | 199 | ROS_ERROR("program stamp 2"); 200 | 201 | while (!g_current_cylinders_initialized) { 202 | ros::Duration(0.5).sleep(); 203 | ros::spinOnce(); 204 | } 205 | ROS_INFO("current_cylinder_blocks initialized"); 206 | 207 | 208 | // the motion planner loop 209 | int count; // the motion count 210 | int cylinder_index; // the index of claimed cylinder 211 | bool left_in_action = false; // left scara boundary status 212 | double left_upper_boundary; 213 | std::vector cylinder_position; // the dynamic cylinder position 214 | cylinder_position.resize(2); 215 | std::vector cylinder_joints; // the dynamic scara joints of cylinder 216 | cylinder_joints.resize(2); 217 | std::vector scara_joints; // the dynamic scara joints position 218 | scara_joints.resize(2); 219 | std::vector scara_position; // the dynamic position of scara 220 | scara_position.resize(2); 221 | std::vector scara_position_new; // the computed new position of scara 222 | scara_position_new.resize(2); 223 | std::vector scara_joints_new; // the computed new joints of scara 224 | scara_joints_new.resize(2); 225 | std::vector cylinder_drop_joints; // cylinder drop position in scara joints 226 | cylinder_drop_joints.resize(2); 227 | std::vector cylinder_drop_position; // cylinder drop position 228 | cylinder_drop_position.resize(2); 229 | std::vector stand_by_joints; // stand by scara joints 230 | stand_by_joints.resize(2); 231 | std::vector stand_by_position; // stand by scara position 232 | stand_by_position.resize(2); 233 | while (ros::ok()) { 234 | // motion has been divided into four parts according to the purposes 235 | 236 | ROS_ERROR("program motion 1 start"); 237 | 238 | // 1.stand by position -> target cylinder position 239 | // claim cylinders from cylinder active pool 240 | bool cylinder_claimed = false; 241 | while (!cylinder_claimed) { 242 | // cylinder not claimed 243 | for (int i=0; i drop position 349 | // update the upper boundary dynamically, set in_action to false at the end 350 | // calculate target position, i.e., drop position 351 | if (g_current_cylinders[cylinder_index] == 0) { 352 | // goes to red cylinder drop position 353 | cylinder_drop_joints[0] = RED_DROP_JOINT1; 354 | cylinder_drop_joints[1] = RED_DROP_JOINT2; 355 | cylinder_drop_position = scara_left_kinematics(cylinder_drop_joints); 356 | } 357 | else { 358 | // goes to blue cylinder drop position 359 | cylinder_drop_joints[0] = BLUE_DROP_JOINT1; 360 | cylinder_drop_joints[1] = BLUE_DROP_JOINT2; 361 | cylinder_drop_position = scara_left_kinematics(cylinder_drop_joints); 362 | } 363 | ROS_INFO_STREAM("drop position: " << cylinder_drop_position[0] << ", " << cylinder_drop_position[1]); 364 | ROS_INFO_STREAM("drop joints: " << cylinder_drop_joints[0] << ", " << cylinder_drop_joints[1]); 365 | count = STEP3_DURANCE / MOTION_SAMPLE_TIME; 366 | for (int i=0; i stand by position 408 | stand_by_joints[0] = STAND_BY_JOINT1; 409 | stand_by_joints[1] = STAND_BY_JOINT2; 410 | stand_by_position = scara_left_kinematics(stand_by_joints); 411 | ROS_INFO_STREAM("stand by position: " << stand_by_position[0] << ", " << stand_by_position[1]); 412 | ROS_INFO_STREAM("stand by joints: " << stand_by_joints[0] << ", " << stand_by_joints[1]); 413 | count = STEP4_DURANCE / MOTION_SAMPLE_TIME; 414 | for (int i=0; i target cylinder position 23 | // 2.target cylinder hovering (for cylinder grasping operation) 24 | // 3.target cylinder position -> drop position 25 | // 4.drop position -> stand by position 26 | // only in motion step 1, the operation may be delayed when the other robot is in operation range 27 | 28 | // upper boundary control method: 29 | // each robot has a state machine contains a boolean and a number 30 | // the boolean indicate if the robot is at upper area of the conveyor 31 | // the number indicate the upper boundary the robot goes 32 | // each robot check the other robot's state and make change to its own 33 | 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | const double CYLINDER_SPEED = 0.12; // the measured absolute speed of the cylinder 48 | const double STEP1_DURANCE = 2.0; // time allocation for step 1 49 | const double STEP2_DURANCE = 4.0; // time allocation for step 2 50 | const double STEP3_DURANCE = 1.0; // time allocation for step 3 51 | const double STEP4_DURANCE = 1.0; // time allocation for step 4 52 | const double MOTION_SAMPLE_TIME = 0.01; 53 | 54 | const double RED_DROP_JOINT1 = 0.3; // drop position for red cylinder 55 | const double RED_DROP_JOINT2 = 2.0; 56 | const double BLUE_DROP_JOINT1 = -1.9; // drop position for blue cylinder 57 | const double BLUE_DROP_JOINT2 = 1.8; 58 | const double STAND_BY_JOINT1 = -0.78; // stand by scara joints 59 | const double STAND_BY_JOINT2 = 2.1; 60 | 61 | const double MAGIC_DISP = 0.03; // real cylinder position compensation 62 | 63 | // global variables 64 | std::vector g_current_cylinders; // 0 or 1 represent red or blue 65 | bool g_current_cylinders_initialized = false; 66 | std::vector g_cylinder_x; // only x y coordinates matter 67 | std::vector g_cylinder_y; 68 | double g_scara_right_r1; // the two joints of scara 69 | double g_scara_right_r2; 70 | std::vector g_cylinder_pool; // the active pool 71 | bool g_left_in_action; // upper boundary status of left scara 72 | double g_left_upper_boundary; 73 | 74 | 75 | // kinematics for the right scara robot 76 | std::vector scara_right_kinematics(std::vector joints) { 77 | // x_generic and y_generic for the generic coordinates used in scara kinematics 78 | double x_generic = cos(joints[0]) + 0.8*cos(joints[0] + joints[1]); 79 | double y_generic = sin(joints[0]) + 0.8*sin(joints[0] + joints[1]); 80 | // convert to coordinates in the view of right scara 81 | std::vector output_position; 82 | output_position.resize(2); 83 | output_position[0] = -y_generic; // x coordinate 84 | output_position[1] = x_generic - 1.5; // y coordinate 85 | return output_position; 86 | } 87 | 88 | // inverse kinematics for the right scara robot 89 | std::vector scara_right_inverse_kinematics(std::vector position) { 90 | // the returned robot configuration is always positive 91 | // convert position to the generic coordinates in scara kinematics 92 | double x_generic = position[1] + 1.5; 93 | double y_generic = -position[0]; 94 | std::vector output_joints; 95 | output_joints.resize(2); 96 | double end_quad_dist = pow(x_generic, 2) + pow(y_generic, 2); 97 | output_joints[0] = atan(y_generic/x_generic) - acos((0.36 + end_quad_dist)/(2*sqrt(end_quad_dist))); 98 | output_joints[1] = M_PI - acos((1.64 - end_quad_dist)/1.6); 99 | // if (abs(output_joints[1]) < 0.2) { 100 | // ROS_INFO_STREAM("IK points: " << x_generic << ", " << y_generic); 101 | // ROS_INFO_STREAM("IK joints: " << output_joints[0] << ", " << output_joints[1]); 102 | // } 103 | // add robust to this inverse kinematics 104 | if ((output_joints[0] != output_joints[0]) || (output_joints[1] != output_joints[1])) { 105 | // there nan in the output, i.e., value inside cos() is out of [-1,1] 106 | output_joints[0] = atan(y_generic/x_generic); 107 | output_joints[1] = 0; 108 | } 109 | return output_joints; 110 | } 111 | 112 | void currentCylinderCallback(const std_msgs::Int8MultiArray& current_cylinder_msg) { 113 | // update current cylinder blocks 114 | if (!g_current_cylinders_initialized) 115 | g_current_cylinders_initialized = true; // set flag to true 116 | g_current_cylinders.resize(current_cylinder_msg.data.size()); 117 | g_current_cylinders = current_cylinder_msg.data; 118 | } 119 | 120 | void cylinderPosesCallback(const two_scara_collaboration::cylinder_blocks_poses& cylinder_poses_msg) { 121 | // update poses message in global variables 122 | int cylinder_quantity = cylinder_poses_msg.x.size(); 123 | g_cylinder_x.resize(cylinder_quantity); 124 | g_cylinder_y.resize(cylinder_quantity); 125 | g_cylinder_x = cylinder_poses_msg.x; 126 | g_cylinder_y = cylinder_poses_msg.y; 127 | } 128 | 129 | void scararightR1PosCallback(const std_msgs::Float64& rotation1_msg) { 130 | // update first joint angle of right scara robot 131 | g_scara_right_r1 = rotation1_msg.data; 132 | } 133 | 134 | void scararightR2PosCallback(const std_msgs::Float64& rotation2_msg) { 135 | // update second joint angle of right scara robot 136 | g_scara_right_r2 = rotation2_msg.data; 137 | } 138 | 139 | void cylinderPoolCallback(const std_msgs::Int8MultiArray& cylinder_pool_msg) { 140 | // update the cylinder active pool 141 | int pool_size = cylinder_pool_msg.data.size(); 142 | g_cylinder_pool.resize(pool_size); 143 | g_cylinder_pool = cylinder_pool_msg.data; 144 | } 145 | 146 | void scaraleftUpperBoundaryCallback(const 147 | two_scara_collaboration::scara_upper_boundary& upper_boundary_msg) { 148 | // update on upper boundary of left scara 149 | g_left_in_action = upper_boundary_msg.in_action; 150 | g_left_upper_boundary = upper_boundary_msg.upper_boundary; 151 | } 152 | 153 | 154 | int main(int argc, char** argv) { 155 | ros::init(argc, argv, "scara_right_motion_planner"); 156 | ros::NodeHandle nh; 157 | 158 | ROS_ERROR("program stamp 1"); 159 | 160 | // initialize subscriber to "/current_cylinder_blocks" 161 | ros::Subscriber current_cylinder_subscriber = nh.subscribe("/current_cylinder_blocks" 162 | , 1, currentCylinderCallback); 163 | // initialize subscriber to "/cylinder_blocks_poses" 164 | ros::Subscriber cylinder_poses_subscriber = nh.subscribe("/cylinder_blocks_poses" 165 | , 1, cylinderPosesCallback); 166 | // initialize subscriber to "/scara_robot_right_rotation1_pos" 167 | ros::Subscriber scara_right_r1_pos_subscriber = nh.subscribe("/scara_robot_right_rotation1_pos" 168 | , 1, scararightR1PosCallback); 169 | // initialize subscriber to "/scara_robot_right_rotation2_pos" 170 | ros::Subscriber scara_right_r2_pos_subscriber = nh.subscribe("/scara_robot_right_rotation2_pos" 171 | , 1, scararightR2PosCallback); 172 | // publish to topic "/scara_robot_right_rotation1_pos_cmd" 173 | ros::Publisher scara_right_r1_cmd_publisher 174 | = nh.advertise("/scara_robot_right_rotation1_pos_cmd", 1); 175 | std_msgs::Float64 scara_right_r1_cmd_msg; 176 | // publish to topic "/scara_robot_right_rotation2_pos_cmd" 177 | ros::Publisher scara_right_r2_cmd_publisher 178 | = nh.advertise("/scara_robot_right_rotation2_pos_cmd", 1); 179 | std_msgs::Float64 scara_right_r2_cmd_msg; 180 | // subscribe to topic "/cylinder_active_pool" 181 | ros::Subscriber cylinder_pool_subscriber = nh.subscribe("/cylinder_active_pool" 182 | , 1, cylinderPoolCallback); 183 | // service client to service "/cylinder_pool_claim" 184 | ros::ServiceClient cylinder_pool_claim_client 185 | = nh.serviceClient("/cylinder_pool_claim"); 186 | two_scara_collaboration::pool_claim_msg cylinder_claim_srv_msg; 187 | // subscribe to topic "/scara_left_upper_boundary" 188 | ros::Subscriber scara_left_upper_boundary_subscriber 189 | = nh.subscribe("/scara_left_upper_boundary", 1, scaraleftUpperBoundaryCallback); 190 | // service client to service "/scara_right_upper_boundary_change" 191 | ros::ServiceClient scara_right_upper_boundary_client 192 | = nh.serviceClient("/scara_right_upper_boundary_change"); 193 | two_scara_collaboration::upper_boundary_change right_boundary_change_srv; 194 | // action client for gripper control, "gripper_action" 195 | actionlib::SimpleActionClient 196 | scara_gripper_action_client("gripper_action", true); 197 | two_scara_collaboration::scara_gripperGoal scara_gripper_goal; 198 | scara_gripper_goal.up_down_left = 0; 199 | scara_gripper_goal.grasp_release_left = 0; 200 | 201 | ROS_ERROR("program stamp 2"); 202 | 203 | while (!g_current_cylinders_initialized) { 204 | ros::Duration(0.5).sleep(); 205 | ros::spinOnce(); 206 | } 207 | ROS_INFO("current_cylinder_blocks initialized"); 208 | 209 | 210 | // the motion planner loop 211 | int count; // the motion count 212 | int cylinder_index; // the index of claimed cylinder 213 | bool right_in_action = false; // right scara boundary status 214 | double right_upper_boundary; 215 | std::vector cylinder_position; // the dynamic cylinder position 216 | cylinder_position.resize(2); 217 | std::vector cylinder_joints; // the dynamic scara joints of cylinder 218 | cylinder_joints.resize(2); 219 | std::vector scara_joints; // the dynamic scara joints position 220 | scara_joints.resize(2); 221 | std::vector scara_position; // the dynamic position of scara 222 | scara_position.resize(2); 223 | std::vector scara_position_new; // the computed new position of scara 224 | scara_position_new.resize(2); 225 | std::vector scara_joints_new; // the computed new joints of scara 226 | scara_joints_new.resize(2); 227 | std::vector cylinder_drop_joints; // cylinder drop position in scara joints 228 | cylinder_drop_joints.resize(2); 229 | std::vector cylinder_drop_position; // cylinder drop position 230 | cylinder_drop_position.resize(2); 231 | std::vector stand_by_joints; // stand by scara joints 232 | stand_by_joints.resize(2); 233 | std::vector stand_by_position; // stand by scara position 234 | stand_by_position.resize(2); 235 | while (ros::ok()) { 236 | // motion has been divided into four parts according to the purposes 237 | 238 | ROS_ERROR("program motion 1 start"); 239 | 240 | // 1.stand by position -> target cylinder position 241 | // claim cylinders from cylinder active pool 242 | bool cylinder_claimed = false; 243 | while (!cylinder_claimed) { 244 | // cylinder not claimed 245 | for (int i=0; i 0.25) // can't reach 247 | continue; 248 | cylinder_claim_srv_msg.request.cylinder_index = g_cylinder_pool[i]; 249 | // call the cylinder claim service 250 | if (cylinder_pool_claim_client.call(cylinder_claim_srv_msg)) { 251 | if (cylinder_claim_srv_msg.response.cylinder_claimed) { 252 | // cylinder has been successfully claimed 253 | cylinder_claimed = true; 254 | cylinder_index = g_cylinder_pool[i]; 255 | break; 256 | } 257 | } 258 | } 259 | if (!cylinder_claimed) 260 | ros::Duration(0.5).sleep(); // sleep for half a second 261 | ros::spinOnce(); 262 | } 263 | ROS_INFO_STREAM("cylinder claimed by right scara: " << cylinder_index); 264 | // check upper boundary status of left scara 265 | if (g_left_in_action) { 266 | // left scara is in action, check boundary value for intersection 267 | // if left_boundary-right_boundary < 0.142, then there is intersection 268 | while ((g_left_upper_boundary - g_cylinder_y[cylinder_index]) < 0.142) { 269 | // there is boundary intersection 270 | ros::Duration(0.1).sleep(); // sleep for 0.1 second 271 | } 272 | } 273 | // no intersection anymore 274 | right_in_action = true; 275 | right_upper_boundary = g_cylinder_y[cylinder_index]; 276 | // send boundary message to boundary topics maintainer 277 | right_boundary_change_srv.request.in_action_change = true; 278 | right_boundary_change_srv.request.in_action = right_in_action; 279 | right_boundary_change_srv.request.upper_boundary_change = true; 280 | right_boundary_change_srv.request.upper_boundary = right_upper_boundary; 281 | scara_right_upper_boundary_client.call(right_boundary_change_srv); 282 | if (right_boundary_change_srv.response.change_is_done) 283 | ROS_INFO_STREAM("right scara upper boundary updated in step 1"); 284 | else 285 | ROS_INFO_STREAM("right scara upper boundary not update in step 1"); 286 | // step 1 motion control 287 | count = STEP1_DURANCE / MOTION_SAMPLE_TIME; 288 | for (int i=0; i drop position 351 | // update the upper boundary dynamically, set in_action to false at the end 352 | // calculate target position, i.e., drop position 353 | if (g_current_cylinders[cylinder_index] == 0) { 354 | // goes to red cylinder drop position 355 | cylinder_drop_joints[0] = RED_DROP_JOINT1; 356 | cylinder_drop_joints[1] = RED_DROP_JOINT2; 357 | cylinder_drop_position = scara_right_kinematics(cylinder_drop_joints); 358 | } 359 | else { 360 | // goes to blue cylinder drop position 361 | cylinder_drop_joints[0] = BLUE_DROP_JOINT1; 362 | cylinder_drop_joints[1] = BLUE_DROP_JOINT2; 363 | cylinder_drop_position = scara_right_kinematics(cylinder_drop_joints); 364 | } 365 | ROS_INFO_STREAM("drop position: " << cylinder_drop_position[0] << ", " << cylinder_drop_position[1]); 366 | ROS_INFO_STREAM("drop joints: " << cylinder_drop_joints[0] << ", " << cylinder_drop_joints[1]); 367 | count = STEP3_DURANCE / MOTION_SAMPLE_TIME; 368 | for (int i=0; i stand by position 410 | stand_by_joints[0] = STAND_BY_JOINT1; 411 | stand_by_joints[1] = STAND_BY_JOINT2; 412 | stand_by_position = scara_right_kinematics(stand_by_joints); 413 | ROS_INFO_STREAM("stand by position: " << stand_by_position[0] << ", " << stand_by_position[1]); 414 | ROS_INFO_STREAM("stand by joints: " << stand_by_joints[0] << ", " << stand_by_joints[1]); 415 | count = STEP4_DURANCE / MOTION_SAMPLE_TIME; 416 | for (int i=0; i