├── README.md ├── Ros ├── BraccioLibRos.cpp ├── BraccioLibRos.h └── src │ ├── CMakeLists.txt │ ├── braccio_pkg │ ├── CMakeLists.txt │ ├── launch │ │ └── unity_simulation_scene.launch │ ├── msg │ │ └── TwistArray.msg │ ├── package.xml │ ├── scripts │ │ ├── __init__.py │ │ ├── publish_angle_node.py │ │ ├── publish_angle_node_Tiwst.py │ │ └── publish_joint_angles.py │ └── setup.py │ ├── file_server │ ├── .gitignore │ ├── CMakeLists.txt │ ├── launch │ │ ├── publish_description_turtlebot2.launch │ │ ├── ros_sharp_communication.launch │ │ └── visualize_robot.launch │ ├── package.xml │ ├── rviz │ │ └── config.rviz │ ├── src │ │ └── file_server.cpp │ └── srv │ │ ├── GetBinaryFile.srv │ │ └── SaveBinaryFile.srv │ ├── gazebo_simulation_scene │ ├── CMakeLists.txt │ ├── launch │ │ └── gazebo_simulation_scene.launch │ ├── package.xml │ └── scripts │ │ └── joy_to_twist.py │ └── unity_simulation_scene │ ├── CMakeLists.txt │ ├── launch │ └── unity_simulation_scene.launch │ ├── package.xml │ └── scripts │ └── mouse_to_joy.py ├── Unity ├── saved_models │ ├── ModelReacher2D.nn │ ├── ModelReacher3D.nn │ ├── ModelReacherRb2D_v1.nn │ ├── ModelReacherRb2D_v2.nn │ └── ModelReacherRb3D_v1.nn ├── scripts_unity │ ├── JointAngleSubscriber.cs │ ├── ReacherAgent2D.cs │ ├── ReacherAgent2DRb_v1.cs │ ├── ReacherAgent2DRb_v2.cs │ ├── ReacherAgent3D.cs │ └── ReacherAgent3DRb_v1.cs └── unity_package │ ├── ReacherAgent.unitypackage │ ├── ReacherAgentRb2D_v1.unitypackage │ ├── ReacherAgentRb2D_v2.unitypackage │ ├── ReacherAgentRb3D_v1.unitypackage │ └── braccio_package.unitypackage ├── WebCam.png ├── braccio_reacher.gif ├── braccio_unity.gif ├── opencv ├── ball_tracking.py ├── balle_braccio.jpg ├── balle_orange.jpg ├── balle_orange_2.jpg ├── camera │ ├── cam_calibration.py │ ├── cam_mtx.txt │ ├── disorsion_param.txt │ ├── scene00001.jpg │ ├── scene00051.jpg │ ├── scene00101.jpg │ ├── scene00151.jpg │ ├── scene00201.jpg │ ├── scene00251.jpg │ ├── scene00301.jpg │ ├── scene00351.jpg │ ├── scene00401.jpg │ ├── scene00451.jpg │ ├── scene00501.jpg │ ├── scene00601.jpg │ ├── scene00651.jpg │ ├── scene00701.jpg │ ├── scene00751.jpg │ ├── scene00801.jpg │ ├── scene00851.jpg │ ├── scene00901.jpg │ ├── scene00951.jpg │ ├── scene01001.jpg │ ├── scene01051.jpg │ └── scene01101.jpg ├── color_boundaries.py ├── test_marker.jpg ├── test_marker.py └── test_seuillage.py └── reacher_rigidBodies.gif /README.md: -------------------------------------------------------------------------------- 1 | # ROS_Unity 2 | 3 | ## Repository with robot simulation with Unity ml-agent and manipulation with ROS 4 | 5 | ### ml-agent is a Unity package which enable the training of agents on Unity physics engine. Linked repo : 6 | 7 | ![](braccio_reacher.gif) 8 | 9 | #### Braccio robot (5 ddl) trained with PPO on a reaching task using Unity Transforms 10 | 11 | ![](reacher_rigidBodies.gif) 12 | 13 | #### Arm robot (3 ddl) trained with PPO on a reaching task using Unity RigidBodies. 14 | 15 | ### Ros# is a Unity/Ros package for communication between Unity simulation scene and ROS/Gazebo. It also enable conversion of urdf files to Unity gameObject. 16 | 17 | ![](braccio_unity.gif) 18 | 19 | #### Braccio robot following the movements of virtual Unity Braccio Robot. Base rotation need to be fixed. 20 | 21 | ![alt text](https://github.com/sabeaussan/ROS_Unity/blob/master/WebCam.png?raw=true) 22 | #### Pose estimation of an Aruco marker 23 | 24 | #### TODO : 25 | 26 | - Fix braccio rotation 27 | - Add sim2real transfer 28 | linked articles : https://arxiv.org/pdf/2002.11635.pdf 29 | https://arxiv.org/pdf/1709.07857.pdf 30 | https://arxiv.org/pdf/1910.07113.pdf 31 | 32 | 33 | -------------------------------------------------------------------------------- /Ros/BraccioLibRos.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Braccio.cpp - board library Version 2.0 3 | Written by Andrea Martino and Angelo Ferrante 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #include "BraccioLibRos.h" 21 | 22 | extern Servo base; 23 | extern Servo shoulder; 24 | extern Servo elbow; 25 | extern Servo wrist_rot; 26 | extern Servo wrist_ver; 27 | extern Servo gripper; 28 | 29 | extern unsigned int step_base = 0; 30 | extern unsigned int step_shoulder = 45; 31 | extern unsigned int step_elbow = 180; 32 | extern unsigned int step_wrist_rot = 180; 33 | extern unsigned int step_wrist_ver = 90; 34 | extern unsigned int step_gripper = 10; 35 | 36 | 37 | 38 | _Braccio Braccio; 39 | 40 | //Initialize Braccio object 41 | _Braccio::_Braccio() { 42 | } 43 | 44 | /** 45 | * Braccio initialization and set intial position 46 | * Modifing this function you can set up the initial position of all the 47 | * servo motors of Braccio 48 | * @param soft_start_level: default value is 0 (SOFT_START_DEFAULT) 49 | * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 50 | * SOFT_START_DISABLED disable the Braccio movements 51 | */ 52 | unsigned int _Braccio::begin(int soft_start_level) { 53 | //Calling Braccio.begin(SOFT_START_DISABLED) the Softstart is disabled and you can use the pin 12 54 | if(soft_start_level!=SOFT_START_DISABLED){ 55 | pinMode(SOFT_START_CONTROL_PIN,OUTPUT); 56 | digitalWrite(SOFT_START_CONTROL_PIN,LOW); 57 | } 58 | 59 | // initialization pin Servo motors 60 | base.attach(11); //M1 61 | shoulder.attach(10); //M2 62 | elbow.attach(9); //M3 63 | wrist_ver.attach(6); //M4 64 | wrist_rot.attach(5); //M5 65 | gripper.attach(3); //M6 66 | 67 | //tastsensor 68 | pinMode(8,INPUT); 69 | 70 | //For each step motor this set up the initial degree 71 | base.write(0); 72 | shoulder.write(40); 73 | elbow.write(180); 74 | wrist_ver.write(0); 75 | wrist_rot.write(170); 76 | gripper.write(73); 77 | //Previous step motor position 78 | step_base = 0; 79 | step_shoulder = 40; 80 | step_elbow = 180; 81 | step_wrist_ver = 0; 82 | step_wrist_rot = 170; 83 | step_gripper = 73; 84 | 85 | if(soft_start_level!=SOFT_START_DISABLED) 86 | _softStart(soft_start_level); 87 | return 1; 88 | } 89 | 90 | /* 91 | Software implementation of the PWM for the SOFT_START_CONTROL_PIN,HIGH 92 | @param high_time: the time in the logic level high 93 | @param low_time: the time in the logic level low 94 | */ 95 | void _Braccio::_softwarePWM(int high_time, int low_time){ 96 | digitalWrite(SOFT_START_CONTROL_PIN,HIGH); 97 | delayMicroseconds(high_time); 98 | digitalWrite(SOFT_START_CONTROL_PIN,LOW); 99 | delayMicroseconds(low_time); 100 | } 101 | 102 | /* 103 | * This function, used only with the Braccio Shield V4 and greater, 104 | * turn ON the Braccio softly and save it from brokes. 105 | * The SOFT_START_CONTROL_PIN is used as a software PWM 106 | * @param soft_start_level: the minimum value is -70, default value is 0 (SOFT_START_DEFAULT) 107 | */ 108 | void _Braccio::_softStart(int soft_start_level){ 109 | long int tmp=millis(); 110 | while(millis()-tmp < LOW_LIMIT_TIMEOUT) 111 | _softwarePWM(80+soft_start_level, 450 - soft_start_level); //the sum should be 530usec 112 | 113 | while(millis()-tmp < HIGH_LIMIT_TIMEOUT) 114 | _softwarePWM(75 + soft_start_level, 430 - soft_start_level); //the sum should be 505usec 115 | 116 | digitalWrite(SOFT_START_CONTROL_PIN,HIGH); 117 | } 118 | 119 | /** 120 | * This functions allow you to control all the servo motors 121 | * 122 | * @param stepDelay The delay between each servo movement 123 | * @param vBase next base servo motor degree 124 | * @param vShoulder next shoulder servo motor degree 125 | * @param vElbow next elbow servo motor degree 126 | * @param vWrist_ver next wrist rotation servo motor degree 127 | * @param vWrist_rot next wrist vertical servo motor degree 128 | * @param vGripper next gripper servo motor degree 129 | */ 130 | void _Braccio::ServoMovement(unsigned int stepDelay, unsigned int vBase, unsigned int vShoulder, unsigned int vElbow, unsigned int vWrist_ver, unsigned int vWrist_rot, unsigned int vGripper) { 131 | vBase=180-vBase; 132 | // Check values, to avoid dangerous positions for the Braccio 133 | if (stepDelay > 30) stepDelay = 30; 134 | if (stepDelay < 10) stepDelay = 10; 135 | if (vBase < 0) vBase=0; 136 | if (vBase > 180) vBase=180; 137 | if (vShoulder < 15) vShoulder=15; 138 | if (vShoulder > 165) vShoulder=165; 139 | if (vElbow < 0) vElbow=0; 140 | if (vElbow > 180) vElbow=180; 141 | if (vWrist_ver < 0) vWrist_ver=0; 142 | if (vWrist_ver > 180) vWrist_ver=180; 143 | if (vWrist_rot > 180) vWrist_rot=180; 144 | if (vWrist_rot < 0) vWrist_rot=0; 145 | if (vGripper < 10) vGripper = 10; 146 | if (vGripper > 73) vGripper = 73; 147 | 148 | int exit = 1; 149 | 150 | //Until the all motors are in the desired position 151 | while (exit) 152 | { 153 | //For each servo motor if next degree is not the same of the previuos than do the movement 154 | if (vBase != step_base) 155 | { 156 | base.write(step_base); 157 | //One step ahead 158 | if (vBase > step_base) { 159 | step_base++; 160 | } 161 | //One step beyond 162 | if (vBase < step_base) { 163 | step_base--; 164 | } 165 | } 166 | 167 | if (vShoulder != step_shoulder) 168 | { 169 | shoulder.write(step_shoulder); 170 | //One step ahead 171 | if (vShoulder > step_shoulder) { 172 | step_shoulder++; 173 | } 174 | //One step beyond 175 | if (vShoulder < step_shoulder) { 176 | step_shoulder--; 177 | } 178 | 179 | } 180 | 181 | if (vElbow != step_elbow) 182 | { 183 | elbow.write(step_elbow); 184 | //One step ahead 185 | if (vElbow > step_elbow) { 186 | step_elbow++; 187 | } 188 | //One step beyond 189 | if (vElbow < step_elbow) { 190 | step_elbow--; 191 | } 192 | 193 | } 194 | 195 | if (vWrist_ver != step_wrist_ver) 196 | { 197 | wrist_ver.write(step_wrist_ver); 198 | //One step ahead 199 | if (vWrist_ver > step_wrist_ver) { 200 | step_wrist_ver++; 201 | } 202 | //One step beyond 203 | if (vWrist_ver < step_wrist_ver) { 204 | step_wrist_ver--; 205 | } 206 | 207 | } 208 | 209 | if (vWrist_rot != step_wrist_rot) 210 | { 211 | wrist_rot.write(step_wrist_rot); 212 | //One step ahead 213 | if (vWrist_rot > step_wrist_rot) { 214 | step_wrist_rot++; 215 | } 216 | //One step beyond 217 | if (vWrist_rot < step_wrist_rot) { 218 | step_wrist_rot--; 219 | } 220 | } 221 | 222 | if (vGripper != step_gripper) 223 | { 224 | gripper.write(step_gripper); 225 | if (vGripper > step_gripper) { 226 | step_gripper++; 227 | } 228 | //One step beyond 229 | if (vGripper < step_gripper) { 230 | step_gripper--; 231 | } 232 | } 233 | 234 | //delay between each movement 235 | delay(stepDelay); 236 | 237 | //It checks if all the servo motors are in the desired position 238 | if ((vBase == step_base) && (vShoulder == step_shoulder) 239 | && (vElbow == step_elbow) && (vWrist_ver == step_wrist_ver) 240 | && (vWrist_rot == step_wrist_rot) && (vGripper == step_gripper)) { 241 | step_base = vBase; 242 | step_shoulder = vShoulder; 243 | step_elbow = vElbow; 244 | step_wrist_ver = vWrist_ver; 245 | step_wrist_rot = vWrist_rot; 246 | step_gripper = vGripper; 247 | exit = 0; 248 | } else { 249 | exit = 1; 250 | } 251 | } 252 | } 253 | 254 | 255 | 256 | 257 | -------------------------------------------------------------------------------- /Ros/BraccioLibRos.h: -------------------------------------------------------------------------------- 1 | /* 2 | Braccio.h - board library Version 2.0 3 | Written by Andrea Martino and Angelo Ferrante 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef BRACCIOLIBROS_H_ 21 | #define BRACCIOLIBROS_H_ 22 | 23 | #include 24 | #include 25 | 26 | // You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 27 | #define SOFT_START_DISABLED -999 28 | 29 | //The default value for the soft start 30 | #define SOFT_START_DEFAULT 0 31 | 32 | //The software PWM is connected to PIN 12. You cannot use the pin 12 if you are using 33 | //a Braccio shield V4 or newer 34 | #define SOFT_START_CONTROL_PIN 12 35 | 36 | //Low and High Limit Timeout for the Software PWM 37 | #define LOW_LIMIT_TIMEOUT 2000 38 | #define HIGH_LIMIT_TIMEOUT 6000 39 | 40 | class _Braccio { 41 | 42 | public: 43 | _Braccio(); 44 | 45 | /** 46 | * Braccio initializations and set intial position 47 | * Modifing this function you can set up the initial position of all the 48 | * servo motors of Braccio 49 | *@param soft_start_level: the minimum value is -70, default value is 0 (SOFT_START_DEFAULT) 50 | * You should set begin(SOFT_START_DISABLED) if you are using the Arm Robot shield V1.6 51 | */ 52 | unsigned int begin(int soft_start_level=SOFT_START_DEFAULT); 53 | 54 | /** 55 | * This function allow the user to control all the servo motors in the Braccio 56 | */ 57 | void ServoMovement(unsigned int stepDelay, unsigned int vBase, unsigned int vShoulder, unsigned int vElbow, unsigned int vWrist_ver, unsigned int vWrist_rot, unsigned int vGripper); 58 | 59 | 60 | 61 | 62 | private: 63 | /* 64 | * This function, used only with the Braccio Shield V4 and greater, 65 | * turn ON the Braccio softly and save Braccio from brokes. 66 | * The SOFT_START_CONTROL_PIN is used as a software PWM 67 | * @param soft_start_level: the minimum value is -70, , default value is 0 (SOFT_START_DEFAULT) 68 | */ 69 | void _softStart(int soft_start_level); 70 | 71 | /* 72 | * Software implementation of the PWM for the SOFT_START_CONTROL_PIN,HIGH 73 | * @param high_time: the time in the logic level high 74 | * @param low_time: the time in the logic level low 75 | */ 76 | void _softwarePWM(int high_time, int low_time); 77 | 78 | 79 | }; 80 | 81 | extern _Braccio Braccio; 82 | 83 | #endif // BRACCIO_H_ 84 | -------------------------------------------------------------------------------- /Ros/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(braccio_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 | rospy 12 | sensor_msgs 13 | std_msgs 14 | geometry_msgs 15 | message_generation 16 | ) 17 | 18 | #catkin_install_python(PROGRAMS scripts/publish_angle_node.py 19 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 20 | #) 21 | 22 | 23 | catkin_python_setup() 24 | ## System dependencies are found with CMake's conventions 25 | # find_package(Boost REQUIRED COMPONENTS system) 26 | 27 | 28 | ## Uncomment this if the package has a setup.py. This macro ensures 29 | ## modules and global scripts declared therein get installed 30 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 31 | # catkin_python_setup() 32 | 33 | ################################################ 34 | ## Declare ROS messages, services and actions ## 35 | ################################################ 36 | 37 | ## To declare and build messages, services or actions from within this 38 | ## package, follow these steps: 39 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 40 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 41 | ## * In the file package.xml: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 45 | ## but can be declared for certainty nonetheless: 46 | ## * add a exec_depend tag for "message_runtime" 47 | ## * In this file (CMakeLists.txt): 48 | ## * add "message_generation" and every package in MSG_DEP_SET to 49 | ## find_package(catkin REQUIRED COMPONENTS ...) 50 | ## * add "message_runtime" and every package in MSG_DEP_SET to 51 | ## catkin_package(CATKIN_DEPENDS ...) 52 | ## * uncomment the add_*_files sections below as needed 53 | ## and list every .msg/.srv/.action file to be processed 54 | ## * uncomment the generate_messages entry below 55 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 56 | 57 | ## Generate messages in the 'msg' folder 58 | add_message_files( 59 | FILES 60 | TwistArray.msg 61 | # Message1.msg 62 | # Message2.msg 63 | ) 64 | 65 | ## Generate services in the 'srv' folder 66 | # add_service_files( 67 | # FILES 68 | # Service1.srv 69 | # Service2.srv 70 | # ) 71 | 72 | ## Generate actions in the 'action' folder 73 | # add_action_files( 74 | # FILES 75 | # Action1.action 76 | # Action2.action 77 | # ) 78 | 79 | ## Generate added messages and services with any dependencies listed here 80 | generate_messages( 81 | DEPENDENCIES 82 | geometry_msgs # Or other packages containing msgs 83 | ) 84 | 85 | ################################################ 86 | ## Declare ROS dynamic reconfigure parameters ## 87 | ################################################ 88 | 89 | ## To declare and build dynamic reconfigure parameters within this 90 | ## package, follow these steps: 91 | ## * In the file package.xml: 92 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 93 | ## * In this file (CMakeLists.txt): 94 | ## * add "dynamic_reconfigure" to 95 | ## find_package(catkin REQUIRED COMPONENTS ...) 96 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 97 | ## and list every .cfg file to be processed 98 | 99 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 100 | # generate_dynamic_reconfigure_options( 101 | # cfg/DynReconf1.cfg 102 | # cfg/DynReconf2.cfg 103 | # ) 104 | 105 | ################################### 106 | ## catkin specific configuration ## 107 | ################################### 108 | ## The catkin_package macro generates cmake config files for your package 109 | ## Declare things to be passed to dependent projects 110 | ## INCLUDE_DIRS: uncomment this if your package contains header files 111 | ## LIBRARIES: libraries you create in this project that dependent projects also need 112 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 113 | ## DEPENDS: system dependencies of this project that dependent projects also need 114 | catkin_package( 115 | # INCLUDE_DIRS include 116 | # LIBRARIES braccio_pkg 117 | CATKIN_DEPENDS rospy message_runtime 118 | # DEPENDS system_lib 119 | ) 120 | 121 | ########### 122 | ## Build ## 123 | ########### 124 | 125 | ## Specify additional locations of header files 126 | ## Your package locations should be listed before other locations 127 | include_directories( 128 | # include 129 | ${catkin_INCLUDE_DIRS} 130 | ) 131 | 132 | ## Declare a C++ library 133 | # add_library(${PROJECT_NAME} 134 | # src/${PROJECT_NAME}/braccio_pkg.cpp 135 | # ) 136 | 137 | ## Add cmake target dependencies of the library 138 | ## as an example, code may need to be generated before libraries 139 | ## either from message generation or dynamic reconfigure 140 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 141 | 142 | ## Declare a C++ executable 143 | ## With catkin_make all packages are built within a single CMake context 144 | ## The recommended prefix ensures that target names across packages don't collide 145 | # add_executable(${PROJECT_NAME}_node src/braccio_pkg_node.cpp) 146 | 147 | ## Rename C++ executable without prefix 148 | ## The above recommended prefix causes long target names, the following renames the 149 | ## target back to the shorter version for ease of user use 150 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 151 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Specify libraries to link a library or executable target against 158 | # target_link_libraries(${PROJECT_NAME}_node 159 | # ${catkin_LIBRARIES} 160 | # ) 161 | 162 | ############# 163 | ## Install ## 164 | ############# 165 | 166 | # all install targets should use catkin DESTINATION variables 167 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 168 | 169 | ## Mark executable scripts (Python etc.) for installation 170 | ## in contrast to setup.py, you can choose the destination 171 | # install(PROGRAMS 172 | # scripts/my_python_script 173 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark executables for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 178 | # install(TARGETS ${PROJECT_NAME}_node 179 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark libraries for installation 183 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 184 | # install(TARGETS ${PROJECT_NAME} 185 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 186 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 187 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 188 | # ) 189 | 190 | ## Mark cpp header files for installation 191 | # install(DIRECTORY include/${PROJECT_NAME}/ 192 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 193 | # FILES_MATCHING PATTERN "*.h" 194 | # PATTERN ".svn" EXCLUDE 195 | # ) 196 | 197 | ## Mark other files for installation (e.g. launch and bag files, etc.) 198 | # install(FILES 199 | # # myfile1 200 | # # myfile2 201 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 202 | # ) 203 | 204 | ############# 205 | ## Testing ## 206 | ############# 207 | 208 | ## Add gtest based cpp test target and link libraries 209 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_braccio_pkg.cpp) 210 | # if(TARGET ${PROJECT_NAME}-test) 211 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 212 | # endif() 213 | 214 | ## Add folders to be run by python nosetests 215 | # catkin_add_nosetests(test) 216 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/launch/unity_simulation_scene.launch: -------------------------------------------------------------------------------- 1 |  15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/msg/TwistArray.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist[] t_array 2 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | braccio_pkg 4 | 0.0.0 5 | The braccio_pkg package 6 | 7 | 8 | 9 | 10 | samuel 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 | rospy 53 | message_generation 54 | message_runtime 55 | sensor_msgs 56 | std_msgs 57 | geometry_msgs 58 | rospy 59 | rospy 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/scripts/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Ros/src/braccio_pkg/scripts/__init__.py -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/scripts/publish_angle_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import random 4 | from sensor_msgs.msg import JointState 5 | from std_msgs.msg import MultiArrayLayout,MultiArrayDimension,UInt8MultiArray 6 | 7 | 8 | def main(): 9 | #angles = rospy.getParam("joint_angles") 10 | angles = [0] *6 11 | pub = rospy.Publisher('joint_array', UInt8MultiArray, queue_size=10) 12 | rospy.init_node('publish_angle_node') 13 | rate = rospy.Rate(0.2) # 10hz 14 | a=0 15 | 16 | while not rospy.is_shutdown(): 17 | array = UInt8MultiArray() 18 | array.data = [] 19 | angles[0] = 90 #random.randrange(1,179) 20 | angles[1] = 90 #random.randrange(16,164) 21 | angles[2] = 90 #random.randrange(1,179) 22 | angles[3] = 90 #random.randrange(1,179) 23 | angles[4] = 90 #random.randrange(1,179) 24 | if a%2==0: 25 | angles[5] = 20 #random.randrange(11,72) 26 | else : 27 | angles[5] = 60 #random.randrange(11,72) 28 | for i in range(6): 29 | array.data.append(angles[i]) 30 | 31 | pub.publish(array); 32 | rate.sleep() 33 | a+=1 34 | 35 | if __name__=="__main__" : 36 | try: 37 | main() 38 | except rospy.ROSInterruptException: 39 | pass 40 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/scripts/publish_angle_node_Tiwst.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import random 4 | from braccio_pkg.msg import TwistArray 5 | #from sensor_msgs.msg import JointState 6 | #from std_msgs.msg import MultiArrayLayout,MultiArrayDimension,UInt8MultiArray 7 | 8 | # Twist ROS to Unity 9 | # z = x 10 | # x = y 11 | # y = z 12 | 13 | def main(): 14 | #angles = rospy.getParam("joint_angles") 15 | angles = [0] *5 16 | pub = rospy.Publisher('joint_angles', TwistArray, queue_size=10) 17 | rospy.init_node('publish_angle_node') 18 | rate = rospy.Rate(0.5) # 2 Hz 19 | array = TwistArray() 20 | angles[0] = Twist() 21 | angles[1] = Twist() 22 | angles[2] = Twist() 23 | angles[3] = Twist() 24 | angles[4] = Twist() 25 | 26 | while not rospy.is_shutdown(): 27 | 28 | array.t_array = [] 29 | angles[0].angular.z = random.randrange(1,179) 30 | angles[1].angular.z = random.randrange(16,164) 31 | angles[2].angular.z = random.randrange(1,179) 32 | angles[3].angular.z = random.randrange(1,179) 33 | angles[4].angular.z = random.randrange(1,179) 34 | 35 | for i in range(5): 36 | array.t_array.append(angles[i]) 37 | 38 | pub.publish(array); 39 | rate.sleep() 40 | a+=1 41 | 42 | if __name__=="__main__" : 43 | try: 44 | main() 45 | except rospy.ROSInterruptException: 46 | pass 47 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/scripts/publish_joint_angles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import random 4 | from std_msgs.msg import MultiArrayLayout,MultiArrayDimension,Float32MultiArray 5 | 6 | 7 | def main(): 8 | angles = [0] *5 9 | pub = rospy.Publisher('joint_angles', Float32MultiArray, queue_size=10) 10 | rospy.init_node('publish_joint_angles') 11 | rate = rospy.Rate(0.3) # 2 Hz 12 | array = Float32MultiArray() 13 | 14 | while not rospy.is_shutdown(): 15 | array.data = [] 16 | angles[0] = random.randrange(0,135) 17 | angles[1] = random.randrange(-35,45) 18 | angles[2] = random.randrange(-55,70) 19 | angles[3] = random.randrange(-55,70) 20 | angles[4] = random.randrange(-55,70) 21 | angles[5] = random.randrange(-55,0) 22 | for i in range(6): 23 | array.data.append(angles[i]) 24 | pub.publish(array); 25 | rate.sleep() 26 | 27 | if __name__=="__main__" : 28 | try: 29 | main() 30 | except rospy.ROSInterruptException: 31 | pass 32 | -------------------------------------------------------------------------------- /Ros/src/braccio_pkg/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 3 | 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['braccio_pkg'], 10 | package_dir={'': 'scripts'} 11 | ) 12 | 13 | setup(**setup_args) 14 | -------------------------------------------------------------------------------- /Ros/src/file_server/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE -------------------------------------------------------------------------------- /Ros/src/file_server/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(file_server) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | message_generation 12 | roslib 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a run_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | # Generate services in the 'srv' folder 56 | add_service_files( 57 | FILES 58 | GetBinaryFile.srv 59 | SaveBinaryFile.srv 60 | ) 61 | 62 | # Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # ) 66 | 67 | # Generate added messages and services with any dependencies listed here 68 | generate_messages( 69 | DEPENDENCIES 70 | std_msgs 71 | ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | LIBRARIES file_services 105 | CATKIN_DEPENDS roscpp rospy std_msgs roslib 106 | DEPENDS message_runtime 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(file_services 122 | # src/${PROJECT_NAME}/file_services.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(file_services ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(file_services_node src/file_services_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(file_services_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(file_services_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS file_services file_services_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_file_services.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | 190 | add_executable(file_server src/file_server.cpp) 191 | target_link_libraries(file_server ${catkin_LIBRARIES}) 192 | add_dependencies(file_server file_server_gencpp) 193 | -------------------------------------------------------------------------------- /Ros/src/file_server/launch/publish_description_turtlebot2.launch: -------------------------------------------------------------------------------- 1 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Ros/src/file_server/launch/ros_sharp_communication.launch: -------------------------------------------------------------------------------- 1 | 2 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /Ros/src/file_server/launch/visualize_robot.launch: -------------------------------------------------------------------------------- 1 | 15 | 16 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /Ros/src/file_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | file_server 4 | 1.0.0 5 | The file server package 6 | 7 | 8 | 9 | 10 | Martin Bischoff 11 | 12 | 13 | 14 | 15 | 16 | Apache License, Version 2.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Martin Bischoff 29 | 30 | 31 | 32 | 33 | 34 | message_generation 35 | 36 | 37 | 38 | message_runtime 39 | 40 | 41 | catkin 42 | roscpp 43 | rospy 44 | std_msgs 45 | roslib 46 | actionlib_msgs 47 | roscpp 48 | rospy 49 | std_msgs 50 | roslib 51 | actionlib_msgs 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /Ros/src/file_server/rviz/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: 565 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 | - Alpha: 1 54 | Class: rviz/RobotModel 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 | base_footprint: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | base_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | camera_depth_frame: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | camera_depth_optical_frame: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | camera_link: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | camera_rgb_frame: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | camera_rgb_optical_frame: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | caster_back_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | caster_front_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | cliff_sensor_front_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | cliff_sensor_left_link: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | cliff_sensor_right_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | gyro_link: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | mount_asus_xtion_pro_link: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | plate_bottom_link: 125 | Alpha: 1 126 | Show Axes: false 127 | Show Trail: false 128 | Value: true 129 | plate_middle_link: 130 | Alpha: 1 131 | Show Axes: false 132 | Show Trail: false 133 | Value: true 134 | plate_top_link: 135 | Alpha: 1 136 | Show Axes: false 137 | Show Trail: false 138 | Value: true 139 | pole_bottom_0_link: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | pole_bottom_1_link: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | pole_bottom_2_link: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | pole_bottom_3_link: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | pole_bottom_4_link: 160 | Alpha: 1 161 | Show Axes: false 162 | Show Trail: false 163 | Value: true 164 | pole_bottom_5_link: 165 | Alpha: 1 166 | Show Axes: false 167 | Show Trail: false 168 | Value: true 169 | pole_kinect_0_link: 170 | Alpha: 1 171 | Show Axes: false 172 | Show Trail: false 173 | Value: true 174 | pole_kinect_1_link: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | pole_middle_0_link: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | pole_middle_1_link: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | Value: true 189 | pole_middle_2_link: 190 | Alpha: 1 191 | Show Axes: false 192 | Show Trail: false 193 | Value: true 194 | pole_middle_3_link: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | Value: true 199 | pole_top_0_link: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | Value: true 204 | pole_top_1_link: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | Value: true 209 | pole_top_2_link: 210 | Alpha: 1 211 | Show Axes: false 212 | Show Trail: false 213 | Value: true 214 | pole_top_3_link: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | Value: true 219 | wheel_left_link: 220 | Alpha: 1 221 | Show Axes: false 222 | Show Trail: false 223 | Value: true 224 | wheel_right_link: 225 | Alpha: 1 226 | Show Axes: false 227 | Show Trail: false 228 | Value: true 229 | Name: RobotModel 230 | Robot Description: robot_description 231 | TF Prefix: "" 232 | Update Interval: 0 233 | Value: true 234 | Visual Enabled: true 235 | Enabled: true 236 | Global Options: 237 | Background Color: 48; 48; 48 238 | Default Light: true 239 | Fixed Frame: base_link 240 | Frame Rate: 30 241 | Name: root 242 | Tools: 243 | - Class: rviz/Interact 244 | Hide Inactive Objects: true 245 | - Class: rviz/MoveCamera 246 | - Class: rviz/Select 247 | - Class: rviz/FocusCamera 248 | - Class: rviz/Measure 249 | - Class: rviz/SetInitialPose 250 | Topic: /initialpose 251 | - Class: rviz/SetGoal 252 | Topic: /move_base_simple/goal 253 | - Class: rviz/PublishPoint 254 | Single click: true 255 | Topic: /clicked_point 256 | Value: true 257 | Views: 258 | Current: 259 | Class: rviz/Orbit 260 | Distance: 2.01816535 261 | Enable Stereo Rendering: 262 | Stereo Eye Separation: 0.0599999987 263 | Stereo Focal Distance: 1 264 | Swap Stereo Eyes: false 265 | Value: false 266 | Focal Point: 267 | X: 0 268 | Y: 0 269 | Z: 0 270 | Focal Shape Fixed Size: true 271 | Focal Shape Size: 0.0500000007 272 | Invert Z Axis: false 273 | Name: Current View 274 | Near Clip Distance: 0.00999999978 275 | Pitch: 0.785398006 276 | Target Frame: 277 | Value: Orbit (rviz) 278 | Yaw: 0.785398006 279 | Saved: ~ 280 | Window Geometry: 281 | Displays: 282 | collapsed: false 283 | Height: 846 284 | Hide Left Dock: false 285 | Hide Right Dock: false 286 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 287 | Selection: 288 | collapsed: false 289 | Time: 290 | collapsed: false 291 | Tool Properties: 292 | collapsed: false 293 | Views: 294 | collapsed: false 295 | Width: 1200 296 | X: 65 297 | Y: 60 298 | -------------------------------------------------------------------------------- /Ros/src/file_server/src/file_server.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | © Siemens AG, 2017-2018 3 | Author: Dr. Martin Bischoff (martin.bischoff@siemens.com) 4 | 5 | Licensed under the Apache License, Version 2.0 (the "License"); 6 | you may not use this file except in compliance with the License. 7 | You may obtain a copy of the License at 8 | . 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | bool get_file( 25 | file_server::GetBinaryFile::Request &req, 26 | file_server::GetBinaryFile::Response &res) 27 | { 28 | 29 | // analyse request: 30 | if (req.name.compare(0,10,"package://")!=0) 31 | { 32 | ROS_INFO("only \"package://\" addresses allowed."); 33 | return true; 34 | } 35 | std::string address = req.name.substr(10); 36 | std::string package = address.substr(0,address.find("/")); 37 | std::string filepath = address.substr(package.length()); 38 | std::string directory = ros::package::getPath(package); 39 | directory+=filepath; 40 | 41 | // open file: 42 | std::ifstream inputfile(directory.c_str(),std::ios::binary); 43 | 44 | // stop if file does not exist: 45 | if(!inputfile.is_open()) 46 | { 47 | ROS_INFO("file \"%s\" not found.\n", req.name.c_str()); 48 | return true; 49 | } 50 | 51 | // read file contents: 52 | res.value.assign( 53 | (std::istreambuf_iterator(inputfile)), 54 | std::istreambuf_iterator()); 55 | 56 | ROS_INFO("get_file request: %s\n", req.name.c_str()); 57 | //ROS_INFO("package: %s\n",package.c_str()); 58 | //ROS_INFO("filepath: %s\n",filepath.c_str()); 59 | //ROS_INFO("directory: %s\n",directory.c_str()); 60 | 61 | return true; 62 | } 63 | 64 | //A recursive method that goes through a path and creates all parent directories 65 | void create_directories(std::string packagePath, std::string filePath) 66 | { 67 | if(filePath.size() == 0) 68 | { 69 | return; 70 | } 71 | 72 | create_directories(packagePath, filePath.substr(0, filePath.find_last_of("/"))); 73 | 74 | struct stat sb; 75 | if (!(stat((packagePath + filePath).c_str(), &sb) == 0 && S_ISDIR(sb.st_mode))) //Check if path is already a directory 76 | { 77 | mkdir((packagePath + filePath).c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); 78 | } 79 | 80 | } 81 | std::string generate_ros_package(std::string packageName) 82 | { 83 | std::string catkin_folder = std::getenv("ROS_PACKAGE_PATH"); 84 | catkin_folder = catkin_folder.substr(0, catkin_folder.find(":")); 85 | 86 | 87 | std::string directory = catkin_folder + "/" + packageName; 88 | mkdir(directory.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); 89 | 90 | //Create default package.xml file 91 | std::string packageXmlContents = "\n\n " + packageName + "\n 0.0.0\n The " + packageName + " package\n\n Ros#\n\n Apache 2.0\n\n catkin\n roscpp\n rospy\n std_msgs\n roscpp\n rospy\n std_msgs\n roscpp\n rospy\n std_msgs\n\n \n \n"; 92 | 93 | std::ofstream xmlfile; 94 | xmlfile.open ((directory + "/package.xml").c_str(), std::ios::out | std::ios::ate | std::ios::binary); 95 | xmlfile << packageXmlContents; 96 | xmlfile.close(); 97 | 98 | //Create default CMakeLists.txt file 99 | std::string cmakelist_content = "cmake_minimum_required(VERSION 2.8.3)\nproject(" + packageName + ")\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rospy\n std_msgs\n)\n\ninclude_directories(\n ${catkin_INCLUDE_DIRS}\n)"; 100 | 101 | std::ofstream cmakefile; 102 | cmakefile.open ((directory + "/CMakeLists.txt").c_str(), std::ios::out | std::ios::ate | std::ios::binary); 103 | cmakefile << cmakelist_content; 104 | cmakefile.close(); 105 | 106 | return directory; 107 | } 108 | 109 | bool save_file( 110 | file_server::SaveBinaryFile::Request &req, 111 | file_server::SaveBinaryFile::Response &res) 112 | { 113 | if (req.name.compare(0,10,"package://")!=0) 114 | { 115 | ROS_INFO("only \"package://\" addresses allowed."); 116 | return true; 117 | } 118 | std::string address = req.name.substr(10); 119 | std::string package = address.substr(0,address.find("/")); 120 | std::string filepath = address.substr(package.length()); 121 | std::string directory = ros::package::getPath(package); 122 | 123 | //Create new ros package if it doesn't already exist 124 | if(directory.compare("") == 0) 125 | { 126 | ROS_INFO("package \"%s\" not found. Creating a new package.", package.c_str()); 127 | directory = generate_ros_package(package); 128 | } 129 | 130 | create_directories(directory, filepath.substr(0, filepath.find_last_of("/"))); 131 | 132 | directory += filepath; 133 | 134 | //Write contents of request to file 135 | std::ofstream file_to_save; 136 | file_to_save.open (directory.c_str(), std::ios::binary); 137 | 138 | std::string str = ""; 139 | for(int i=0; i < req.value.size(); i++){ str += req.value[i]; } 140 | file_to_save << str; 141 | file_to_save.close(); 142 | 143 | ROS_INFO("save_file request: %s", req.name.c_str()); 144 | //ROS_INFO("package: %s\n",package.c_str()); 145 | //ROS_INFO("filepath: %s\n",filepath.c_str()); 146 | //ROS_INFO("directory: %s\n",directory.c_str()); 147 | 148 | res.name = req.name; 149 | 150 | return true; 151 | } 152 | int main(int argc, char **argv) 153 | { 154 | ros::init(argc, argv, "file_server"); 155 | ros::NodeHandle n; 156 | ros::ServiceServer serviceServer = n.advertiseService("/file_server/get_file", get_file); 157 | ros::ServiceServer serviceServer2 = n.advertiseService("/file_server/save_file", save_file); 158 | ROS_INFO("ROSbridge File Server initialized."); 159 | ros::spin(); 160 | 161 | return 0; 162 | } 163 | -------------------------------------------------------------------------------- /Ros/src/file_server/srv/GetBinaryFile.srv: -------------------------------------------------------------------------------- 1 | string name 2 | --- 3 | uint8[] value 4 | 5 | -------------------------------------------------------------------------------- /Ros/src/file_server/srv/SaveBinaryFile.srv: -------------------------------------------------------------------------------- 1 | string name 2 | uint8[] value 3 | --- 4 | string name 5 | -------------------------------------------------------------------------------- /Ros/src/gazebo_simulation_scene/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gazebo_simulation_scene) 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 | geometry_msgs 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | # add_service_files( 60 | # FILES 61 | # Service1.srv 62 | # Service2.srv 63 | # ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | # generate_messages( 74 | # DEPENDENCIES 75 | # geometry_msgs# sensor_msgs# std_msgs 76 | # ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES gazebo_simulation_scene 110 | # CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/gazebo_simulation_scene.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/gazebo_simulation_scene_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # install(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables and/or libraries for installation 170 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 171 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark cpp header files for installation 177 | # install(DIRECTORY include/${PROJECT_NAME}/ 178 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 179 | # FILES_MATCHING PATTERN "*.h" 180 | # PATTERN ".svn" EXCLUDE 181 | # ) 182 | 183 | ## Mark other files for installation (e.g. launch and bag files, etc.) 184 | # install(FILES 185 | # # myfile1 186 | # # myfile2 187 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 188 | # ) 189 | 190 | ############# 191 | ## Testing ## 192 | ############# 193 | 194 | ## Add gtest based cpp test target and link libraries 195 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_gazebo_simulation_scene.cpp) 196 | # if(TARGET ${PROJECT_NAME}-test) 197 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 198 | # endif() 199 | 200 | ## Add folders to be run by python nosetests 201 | # catkin_add_nosetests(test) 202 | -------------------------------------------------------------------------------- /Ros/src/gazebo_simulation_scene/launch/gazebo_simulation_scene.launch: -------------------------------------------------------------------------------- 1 |  15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /Ros/src/gazebo_simulation_scene/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_simulation_scene 4 | 0.0.0 5 | The gazebo_simulation_scene package 6 | 7 | 8 | 9 | 10 | Berkay Alp Cakal 11 | 12 | 13 | 14 | 15 | 16 | Apache License, Version 2.0 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 | geometry_msgs 53 | roscpp 54 | rospy 55 | sensor_msgs 56 | std_msgs 57 | geometry_msgs 58 | roscpp 59 | rospy 60 | sensor_msgs 61 | std_msgs 62 | geometry_msgs 63 | roscpp 64 | rospy 65 | sensor_msgs 66 | std_msgs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /Ros/src/gazebo_simulation_scene/scripts/joy_to_twist.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Siemens AG, 2018 4 | # Author: Berkay Alp Cakal (berkay_alp.cakal.ct@siemens.com) 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # . 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import numpy 17 | from sensor_msgs.msg import Joy 18 | from geometry_msgs.msg import Twist 19 | 20 | 21 | def JoyToTwist(): 22 | # initialize node 23 | rospy.init_node('JoyToTwist', anonymous=True) 24 | 25 | # setup joy topic subscription 26 | joy_subscriber = rospy.Subscriber("joy", Joy, handleJoyMsg, queue_size=10) 27 | 28 | # spin() simply keeps python from exiting until this node is stopped 29 | rospy.spin() 30 | 31 | 32 | def handleJoyMsg(data): 33 | #### Initialize Speed Parameter 34 | # axes [l.x l.y l.z a.x a.y a.z] 35 | scalers = [0.7, 0.7, 0.7, -3.14, -3.14, -3.14] 36 | 37 | 38 | #### Setup Twist Publisher 39 | twist_publisher = rospy.Publisher("cmd_vel_mux/input/navi", Twist) 40 | msg = Twist() 41 | 42 | #### Start Mapping from Joy to Twist 43 | if len(data.axes) >= 6 : 44 | msg.angular.x = data.axes[5] * scalers[3] 45 | 46 | if len(data.axes) >= 5 : 47 | msg.linear.z = data.axes[4] * scalers[2] 48 | 49 | if len(data.axes) >= 4 : 50 | msg.angular.y = data.axes[3] * scalers[4] 51 | 52 | if len(data.axes) >= 3 : 53 | msg.linear.y = data.axes[2] * scalers[1] 54 | 55 | if len(data.axes) >= 2 : 56 | msg.angular.z = data.axes[1] * scalers[5] 57 | 58 | if len(data.axes) >= 1 : 59 | msg.linear.x = data.axes[0] * scalers[0] 60 | 61 | #### Publish msg 62 | rate = rospy.Rate(100) # 10hz 63 | rospy.loginfo(msg) 64 | twist_publisher.publish(msg) 65 | rate.sleep() 66 | 67 | if __name__ == '__main__': 68 | JoyToTwist() 69 | -------------------------------------------------------------------------------- /Ros/src/unity_simulation_scene/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unity_simulation_scene) 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 run_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 run_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 run_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 unity_simulation_scene 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}/unity_simulation_scene.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/unity_simulation_scene_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 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_unity_simulation_scene.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | -------------------------------------------------------------------------------- /Ros/src/unity_simulation_scene/launch/unity_simulation_scene.launch: -------------------------------------------------------------------------------- 1 |  15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /Ros/src/unity_simulation_scene/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unity_simulation_scene 4 | 0.0.0 5 | The unity_simulation_scene package 6 | 7 | 8 | 9 | 10 | Berkay Alp Cakal 11 | 12 | 13 | 14 | 15 | 16 | Apache License, Version 2.0 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 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /Ros/src/unity_simulation_scene/scripts/mouse_to_joy.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Siemens AG, 2018 4 | # Author: Berkay Alp Cakal (berkay_alp.cakal.ct@siemens.com) 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # . 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import numpy 17 | from sensor_msgs.msg import Joy 18 | from geometry_msgs.msg import Twist 19 | from Xlib import display 20 | from Xlib.ext import randr 21 | 22 | def mouseToJoy(): 23 | # initialize node 24 | rospy.init_node('mouseToJoy', anonymous = True) 25 | 26 | #### Setup MouseToJoy Publisher 27 | mouseToJoyPublisher = rospy.Publisher("joy", Joy, queue_size = 5) 28 | rate = rospy.Rate(10) # 10hz 29 | msg = Joy() 30 | 31 | while not rospy.is_shutdown(): 32 | #### Initialize joy msg every loop 33 | msg.axes = [] 34 | msg.buttons = [] 35 | pos_x = 0.0 36 | pos_y = 0.0 37 | 38 | #### Get Display Dependent Parameters 39 | d = display.Display() 40 | screen = d.screen() 41 | window = screen.root.create_window(0, 0, 1, 1, 1, screen.root_depth) 42 | res = randr.get_screen_resources(window) 43 | 44 | resolution_x = res.modes[0].width # i.e. 1920 45 | resolution_y = res.modes[0].height # i.e. 1080 46 | middlePoint_x = resolution_x / 2.0 47 | middlePoint_y = resolution_y / 2.0 48 | 49 | #### Start Getting Postion of Mouse 50 | MouseData = display.Display().screen().root.query_pointer()._data 51 | pos_x = MouseData["root_x"] 52 | pos_y = MouseData["root_y"] 53 | 54 | #### Start Mapping from Mouse Position to Joy 55 | vel_linear = (pos_y - middlePoint_y) / resolution_y * (2) 56 | vel_angular = (pos_x - middlePoint_x) / resolution_x * (2) 57 | 58 | msg.axes.append(vel_linear) 59 | msg.axes.append(vel_angular) 60 | 61 | 62 | #### Publish msg 63 | #rospy.loginfo([pos_x, pos_y]) 64 | rospy.loginfo(msg) 65 | mouseToJoyPublisher.publish(msg) 66 | #rate.sleep() 67 | 68 | 69 | if __name__ == '__main__': 70 | mouseToJoy() 71 | -------------------------------------------------------------------------------- /Unity/saved_models/ModelReacher2D.nn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/saved_models/ModelReacher2D.nn -------------------------------------------------------------------------------- /Unity/saved_models/ModelReacher3D.nn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/saved_models/ModelReacher3D.nn -------------------------------------------------------------------------------- /Unity/saved_models/ModelReacherRb2D_v1.nn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/saved_models/ModelReacherRb2D_v1.nn -------------------------------------------------------------------------------- /Unity/saved_models/ModelReacherRb2D_v2.nn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/saved_models/ModelReacherRb2D_v2.nn -------------------------------------------------------------------------------- /Unity/saved_models/ModelReacherRb3D_v1.nn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/saved_models/ModelReacherRb3D_v1.nn -------------------------------------------------------------------------------- /Unity/scripts_unity/JointAngleSubscriber.cs: -------------------------------------------------------------------------------- 1 | 2 | 3 | using UnityEngine; 4 | 5 | namespace RosSharp.RosBridgeClient 6 | { 7 | public class JointAngleSubscriber : UnitySubscriber 8 | { 9 | public Transform SubscribedTransform; 10 | public int id; 11 | public int axis_x; 12 | public int axis_y; 13 | public int axis_z; 14 | private int robot_speed = 2; 15 | 16 | private float desired_angle; 17 | private float new_angle; 18 | private int sign; 19 | private bool stop; 20 | private float oldDesiredAngle = 0f; 21 | 22 | protected override void Start() 23 | { 24 | base.Start(); 25 | stop = true; 26 | new_angle=0f; 27 | } 28 | 29 | private void Update() 30 | { 31 | if(!stop) 32 | ProcessMessage(); 33 | } 34 | 35 | protected override void ReceiveMessage(MessageTypes.Std.Float32MultiArray message) 36 | { 37 | desired_angle = message.data[id];//.Ros2Unity(); 38 | stop = oldDesiredAngle == desired_angle; 39 | } 40 | 41 | private void ProcessMessage() 42 | { 43 | if(new_angle < desired_angle ){ 44 | sign = 1; 45 | } 46 | else{ 47 | sign = -1; 48 | } 49 | if(id == 4) { 50 | Debug.Log( "id : "+id+ " sign : " +sign); 51 | } 52 | /*SubscribedTransform.Rotate( 53 | robot_speed * axis_x * sign, 54 | robot_speed * axis_y * sign, 55 | robot_speed * axis_z * sign 56 | );*/ 57 | new_angle = new_angle + robot_speed * sign; 58 | SubscribedTransform.localRotation = Quaternion.Euler(new_angle * axis_x ,new_angle * axis_y, axis_z * new_angle); 59 | if(id == 4) { 60 | Debug.Log( "id : "+id+ " new_angle : " +new_angle); 61 | Debug.Log( "id : "+id+ " sign : " +sign); 62 | } 63 | if(sign == 1){ 64 | stop = new_angle >= desired_angle; 65 | oldDesiredAngle = desired_angle; 66 | } 67 | else{ 68 | stop = new_angle <= desired_angle; 69 | oldDesiredAngle = desired_angle; 70 | } 71 | if(id == 4) 72 | Debug.Log( "id : "+id+ " stop : " +stop); 73 | } 74 | 75 | } 76 | } -------------------------------------------------------------------------------- /Unity/scripts_unity/ReacherAgent2D.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using MLAgents; 3 | using MLAgents.Sensors; 4 | 5 | public class ReacherAgent2D : Agent 6 | { 7 | // Game Object 8 | public GameObject segment1; 9 | public GameObject segment2; 10 | public GameObject hand; 11 | public GameObject arm_base; 12 | public GameObject goal; 13 | 14 | // Variable d'entrainement 15 | public float goal_speed; // Vitesse de déplacement de la balle 16 | public int dev_freq; // Fréquence à laquelle la balle change de direction (en step de simu) 17 | public float positive_range; // distance hand-goal à partir de laquelle la récompense est >0 18 | public float alpha; // constante pour régler la reward positive 19 | 20 | 21 | private float SEGMENT1_LENGTH; // longueur du premier segment du bras 22 | private Vector3 ORIGIN; // position absolue de la base 23 | private static float ARM_RANGE = 4.5f; // Longueur total du bras + hand 24 | 25 | // Angle pour la position du goal 26 | private float m_Goal_phi; 27 | // rayon du cercle sur lequel se trouve le goal 28 | private float m_Goal_rho; 29 | 30 | 31 | // Signe pour le déplacement du goal 32 | private int sign_x; 33 | private int sign_y; 34 | 35 | private float episode_reward; 36 | private int nb_step; 37 | 38 | 39 | 40 | 41 | public override void Initialize() 42 | { 43 | episode_reward = 0f; 44 | SEGMENT1_LENGTH = segment2.transform.position.y; 45 | ORIGIN = arm_base.transform.position; 46 | 47 | } 48 | 49 | 50 | 51 | 52 | // Resets the position of the agent and the goal. 53 | public override void OnEpisodeBegin() 54 | { 55 | 56 | nb_step = 0; 57 | 58 | // Place la sphère à atteindre à une position random 59 | do{ 60 | sign_x = Random.Range(-1,2); 61 | sign_y = Random.Range(-1,2); 62 | }while(sign_x == 0 && sign_y == 0); 63 | episode_reward = 0f; 64 | SpawnGoal(); 65 | 66 | 67 | } 68 | 69 | public void UpdateGoalPosition(){ 70 | nb_step++; 71 | if(nb_step%dev_freq ==0){ 72 | // Change la direction de la balle fréquemment 73 | sign_x = Random.Range(-1,2); 74 | sign_y = Random.Range(-1,2); 75 | } 76 | 77 | // On check si bouger le goal ne le fait pas sortir des limites 78 | Vector3 new_pos_x = goal.transform.localPosition + new Vector3(goal_speed * sign_x,0f,0f); 79 | Vector3 new_pos_y = goal.transform.localPosition + new Vector3(0f,goal_speed*sign_y,0f); 80 | float distance_x = Vector3.Distance(new_pos_x,arm_base.transform.localPosition); 81 | float distance_y = Vector3.Distance(new_pos_y,arm_base.transform.localPosition); 82 | ; if( distance_x > ARM_RANGE - 0.1f || distance_x < 1.5f ){ 83 | sign_x = -sign_x; 84 | } 85 | if( distance_y > ARM_RANGE - 0.1f || arm_base.transform.localPosition.y -0.25f > new_pos_y.y || distance_y < 1.5f ){ 86 | sign_y = -sign_y; 87 | } 88 | Vector3 increment = new Vector3(goal_speed * sign_x,goal_speed*sign_y,0f); 89 | goal.transform.localPosition = increment + goal.transform.localPosition; 90 | 91 | } 92 | 93 | public void SpawnGoal(){ 94 | m_Goal_phi = Random.Range(0, Mathf.PI); 95 | m_Goal_rho = Random.Range(2,ARM_RANGE); 96 | 97 | 98 | // 2D 99 | var goalX = m_Goal_rho * Mathf.Cos(m_Goal_phi); 100 | var goalY = m_Goal_rho * Mathf.Sin(m_Goal_phi); 101 | 102 | 103 | goal.transform.localPosition = new Vector3(goalX, goalY, 0f); 104 | } 105 | 106 | public override void CollectObservations(VectorSensor sensor) 107 | { 108 | 109 | 110 | sensor.AddObservation(segment1.transform.rotation); //4 111 | sensor.AddObservation(segment2.transform.rotation); //4 112 | 113 | 114 | Vector3 goal_pos = goal.transform.position - ORIGIN; // Ramène les coordonnées au centre de la scene 115 | Vector3 hand_pos = hand.transform.position - ORIGIN; // Ramène les coordonnées au centre de la scene 116 | sensor.AddObservation(goal_pos); //3 117 | sensor.AddObservation(hand_pos); //3 118 | 119 | } 120 | 121 | // The agent's four actions correspond to torques on each of the two joints. 122 | public override void OnActionReceived(float[] vectorAction) 123 | { 124 | float reward_obtained; 125 | 126 | // Rotation du segment 1 127 | float consigne_1 = Mathf.Clamp(vectorAction[0], -1f, 1f) * 90; 128 | segment1.transform.rotation = Quaternion.Euler(0f,0f,consigne_1); 129 | 130 | // Rotation du segment 2 131 | float consigne_2 = Mathf.Clamp(vectorAction[1], -1f, 1f) * 155 + consigne_1; 132 | segment2.transform.rotation = Quaternion.Euler(0f,0f,consigne_2); 133 | consigne_1 = consigne_1 * Mathf.PI/180f; 134 | 135 | float pos_y = SEGMENT1_LENGTH * Mathf.Cos(consigne_1); 136 | float pos_x = SEGMENT1_LENGTH * Mathf.Sin(consigne_1); 137 | 138 | segment2.transform.localPosition = new Vector3(-pos_x,pos_y + 0.5f,0f); 139 | UpdateGoalPosition(); 140 | 141 | float distance_to_target = Vector3.Distance(hand.transform.position,goal.transform.position); 142 | 143 | if(distance_to_target < positive_range){ 144 | reward_obtained = 1/(distance_to_target * alpha); 145 | } 146 | else { 147 | reward_obtained = -1.0f * distance_to_target; 148 | 149 | } 150 | SetReward(reward_obtained); 151 | episode_reward += reward_obtained; 152 | 153 | } 154 | 155 | 156 | 157 | public override float[] Heuristic() 158 | { 159 | var action = new float[2]; 160 | action[0] = Input.GetAxis("Horizontal"); 161 | action[1] = Input.GetAxis("Vertical"); 162 | 163 | return action; 164 | } 165 | 166 | 167 | 168 | 169 | } 170 | -------------------------------------------------------------------------------- /Unity/scripts_unity/ReacherAgent2DRb_v1.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using MLAgents; 3 | using MLAgents.Sensors; 4 | 5 | // TODO : convertir consigne en angle pour l'observation 6 | 7 | public class ReacherAgent : Agent 8 | { 9 | public GameObject segment1; 10 | public GameObject segment2; 11 | public GameObject hand; 12 | public GameObject arm_base; 13 | public GameObject joint; 14 | public GameObject goal; 15 | public float goal_speed; 16 | public int dev_freq; 17 | public float positive_range; 18 | 19 | 20 | private float SEGMENT1_LENGTH; 21 | private Vector3 ORIGIN; 22 | private static float ARM_RANGE = 4.4f; 23 | 24 | private float m_Goal_theta; 25 | private float m_Goal_phi; 26 | private float m_Goal_rho; 27 | 28 | private float episode_reward; 29 | private int sign_x; 30 | private int sign_y; 31 | private int sign_z; 32 | private int nb_step; 33 | 34 | private Rigidbody rb1; 35 | private Rigidbody rb2; 36 | private Rigidbody rbb; 37 | private ConfigurableJoint config_joint; 38 | 39 | // Collect the rigidbodies of the reacher in order to resue them for 40 | // observations and actions. 41 | public override void Initialize() 42 | { 43 | episode_reward = 0f; 44 | SEGMENT1_LENGTH = segment2.transform.position.y; 45 | ORIGIN = arm_base.transform.position; 46 | rb1 = segment1.GetComponent(); 47 | rb2 = segment2.GetComponent(); 48 | rbb = arm_base.GetComponent(); 49 | config_joint = segment1.GetComponent(); 50 | } 51 | 52 | 53 | 54 | 55 | // Resets the position of the agent and the goal. 56 | public override void OnEpisodeBegin() 57 | { 58 | 59 | nb_step = 0; 60 | // Place la sphère à atteindre à une position 61 | // Calcul fait en coordonnées sphériques 62 | 63 | do{ 64 | sign_x = Random.Range(-1,2); 65 | sign_y = Random.Range(-1,2); 66 | //sign_z = Random.Range(-1,2); 67 | }while(sign_x == 0 && sign_y == 0 /*&& sign_z==0*/); 68 | 69 | episode_reward = 0f; 70 | SpawnGoal(); 71 | 72 | 73 | } 74 | 75 | public void UpdateGoalPosition(){ 76 | float MIN_RANGE = 1.5f; 77 | nb_step++; 78 | if(nb_step%dev_freq ==0){ 79 | sign_x = Random.Range(-1,2); 80 | sign_y = Random.Range(-1,2); 81 | //sign_z = Random.Range(-1,2); 82 | } 83 | Vector3 new_pos_x = goal.transform.localPosition + new Vector3(goal_speed * sign_x,0f,0f); 84 | Vector3 new_pos_y = goal.transform.localPosition + new Vector3(0f,goal_speed*sign_y,0f); 85 | //Vector3 new_pos_z = goal.transform.localPosition + new Vector3(0f,0f,goal_speed*sign_z); 86 | 87 | 88 | float distance_x = Vector3.Distance(new_pos_x,arm_base.transform.localPosition); 89 | float distance_y = Vector3.Distance(new_pos_y,arm_base.transform.localPosition); 90 | //float distance_z = Vector3.Distance(new_pos_z,arm_base.transform.localPosition); 91 | 92 | // Condition limite 93 | if( distance_x > ARM_RANGE - 0.1f || distance_x < MIN_RANGE ){ 94 | sign_x = -sign_x; 95 | } 96 | /*if( distance_z > ARM_RANGE - 0.1f || distance_z < MIN_RANGE ){ 97 | sign_z = -sign_z; 98 | }*/ 99 | if( distance_y > ARM_RANGE - 0.1f || arm_base.transform.localPosition.y -0.25f > new_pos_y.y || distance_y < MIN_RANGE ){ 100 | sign_y = -sign_y; 101 | } 102 | Vector3 increment = new Vector3(goal_speed * sign_x,goal_speed*sign_y,0f); 103 | goal.transform.localPosition = increment + goal.transform.localPosition; 104 | 105 | } 106 | 107 | public void SpawnGoal(){ 108 | m_Goal_theta = Random.Range(0, Mathf.PI/ 2f); 109 | m_Goal_phi = Random.Range(0, Mathf.PI); 110 | m_Goal_rho = Random.Range(2,ARM_RANGE); 111 | 112 | 113 | // Sphérique 114 | var goalX = m_Goal_rho * Mathf.Sin(m_Goal_theta) /** Mathf.Cos(m_Goal_phi)*/; 115 | //var goalZ = m_Goal_rho * Mathf.Sin(m_Goal_theta) * Mathf.Sin(m_Goal_phi) ; 116 | var goalY = m_Goal_rho * Mathf.Cos(m_Goal_theta); 117 | 118 | 119 | goal.transform.localPosition = new Vector3(goalX, goalY, 0f); 120 | } 121 | 122 | // We collect the normalized rotations, angularal velocities, and velocities of both 123 | // limbs of the reacher as well as the relative position of the target and hand. 124 | public override void CollectObservations(VectorSensor sensor) 125 | { 126 | var angle_S1 = segment1.transform.rotation.eulerAngles.z; 127 | var angle_S2 = segment2.transform.rotation.eulerAngles.z; 128 | if(segment1.transform.rotation.eulerAngles.z > 120f){ 129 | angle_S1 = segment1.transform.rotation.eulerAngles.z - 360f; 130 | } 131 | if(segment2.transform.rotation.eulerAngles.z > 190f){ 132 | angle_S2 = segment2.transform.rotation.eulerAngles.z - 360f; 133 | angle_S2 = angle_S2 - angle_S1; 134 | } 135 | 136 | //Debug.Log("angle_S1 : "+angle_S1); 137 | //Debug.Log("angularVelocity 1 : "+rb1.angularVelocity); 138 | //Debug.Log("Velocity 1 : "+rb1.velocity); 139 | sensor.AddObservation(angle_S1); //1 140 | sensor.AddObservation(rb1.angularVelocity); //3 141 | sensor.AddObservation(rb1.velocity); //3 142 | 143 | //Debug.Log("angle_S2 : "+angle_S2); 144 | //Debug.Log("angularVelocity 2 : "+rb2.angularVelocity); 145 | //Debug.Log("Velocity 2 : "+rb2.velocity); 146 | sensor.AddObservation(angle_S2); //1 147 | sensor.AddObservation(rb2.angularVelocity); //3 148 | sensor.AddObservation(rb2.velocity); //3 149 | 150 | 151 | //sensor.AddObservation(arm_base.transform.rotation.eulerAngles.y); //1 152 | //sensor.AddObservation(rbb.angularVelocity.y); //1 153 | 154 | Vector3 goal_pos = goal.transform.position - ORIGIN; 155 | Vector3 hand_pos = hand.transform.position - ORIGIN; 156 | sensor.AddObservation(goal_pos); //3 157 | sensor.AddObservation(hand_pos); //3 158 | //sensor.AddObservation(goal_speed); //1 159 | 160 | // dim_obs = 23 161 | 162 | } 163 | 164 | // The agent's four actions correspond to torques on each of the two joints. 165 | public override void OnActionReceived(float[] vectorAction) 166 | { 167 | float reward_obtained; 168 | //config_joint.angularZMotion = ConfigurableJointMotion.Free; 169 | 170 | 171 | // Rotation segment 2 172 | var torque = Mathf.Clamp(vectorAction[1], -1f, 1f) * 10f; 173 | Debug.Log("torque s2: "+ vectorAction[1]); 174 | if(torque == 0){ 175 | //rb2.constraints |= RigidbodyConstraints.FreezeRotationZ; 176 | //config_joint.angularZMotion = ConfigurableJointMotion.Locked; 177 | } 178 | else { 179 | //rb2.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 180 | //config_joint.angularZMotion = ConfigurableJointMotion.Free; 181 | } 182 | rb2.AddRelativeTorque(new Vector3(0f, 0f, torque)); 183 | 184 | 185 | // Rotation segment 1 186 | torque = Mathf.Clamp(vectorAction[0], -1f, 1f) * 10f; 187 | Debug.Log("torque s1: "+ vectorAction[0]); 188 | if(torque == 0){ 189 | //rb1.constraints |= RigidbodyConstraints.FreezeRotationZ; 190 | } 191 | else { 192 | //rb1.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 193 | } 194 | rb1.AddRelativeTorque(new Vector3(0f, 0f, torque)); 195 | 196 | 197 | // Rotation base 198 | /*torque = Mathf.Clamp(vectorAction[2], -1f, 1f) * 10f; 199 | Debug.Log("torque base: "+ torque); 200 | if(torque == 0){ 201 | rbb.constraints |= RigidbodyConstraints.FreezeRotationY; 202 | } 203 | else { 204 | rbb.constraints &= ~ RigidbodyConstraints.FreezeRotationY; 205 | } 206 | rbb.AddRelativeTorque(new Vector3(0f,torque,0f));*/ 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | UpdateGoalPosition(); 215 | 216 | float distance_to_target = Vector3.Distance(hand.transform.position,goal.transform.position); 217 | 218 | if(distance_to_target < positive_range){ 219 | reward_obtained = 0.1f; 220 | } 221 | else { 222 | reward_obtained = -1.0f * distance_to_target; 223 | 224 | } 225 | SetReward(reward_obtained); 226 | 227 | } 228 | 229 | 230 | 231 | public override float[] Heuristic() 232 | { 233 | var action = new float[3]; 234 | action[0] = Input.GetAxis("Horizontal"); 235 | action[1] = Input.GetAxis("Vertical"); 236 | //action[2] = Input.GetAxis("Rotation"); 237 | 238 | return action; 239 | } 240 | 241 | //public addFixedJoint(GameObject obj){ 242 | // obj. 243 | //} 244 | 245 | 246 | } 247 | -------------------------------------------------------------------------------- /Unity/scripts_unity/ReacherAgent2DRb_v2.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using MLAgents; 3 | using MLAgents.Sensors; 4 | 5 | // TODO : convertir consigne en angle pour l'observation 6 | 7 | public class ReacherAgent : Agent 8 | { 9 | public GameObject segment1; 10 | public GameObject segment2; 11 | public GameObject hand; 12 | public GameObject arm_base; 13 | public GameObject joint; 14 | public GameObject goal; 15 | public float goal_speed; 16 | public int dev_freq; 17 | public float positive_range; 18 | 19 | 20 | private float SEGMENT1_LENGTH; 21 | private Vector3 ORIGIN; 22 | private static float ARM_RANGE = 4.4f; 23 | 24 | private float m_Goal_theta; 25 | private float m_Goal_phi; 26 | private float m_Goal_rho; 27 | 28 | private float episode_reward; 29 | private int sign_x; 30 | private int sign_y; 31 | private int sign_z; 32 | private int nb_step; 33 | 34 | private Rigidbody rb1; 35 | private Rigidbody rb2; 36 | private Rigidbody rbb; 37 | private ConfigurableJoint config_joint; 38 | 39 | // Collect the rigidbodies of the reacher in order to resue them for 40 | // observations and actions. 41 | public override void Initialize() 42 | { 43 | episode_reward = 0f; 44 | SEGMENT1_LENGTH = segment2.transform.position.y; 45 | ORIGIN = arm_base.transform.position; 46 | rb1 = segment1.GetComponent(); 47 | rb2 = segment2.GetComponent(); 48 | rbb = arm_base.GetComponent(); 49 | config_joint = segment1.GetComponent(); 50 | } 51 | 52 | 53 | 54 | 55 | // Resets the position of the agent and the goal. 56 | public override void OnEpisodeBegin() 57 | { 58 | 59 | nb_step = 0; 60 | // Place la sphère à atteindre à une position 61 | // Calcul fait en coordonnées sphériques 62 | 63 | do{ 64 | sign_x = Random.Range(-1,2); 65 | sign_y = Random.Range(-1,2); 66 | }while(sign_x == 0 && sign_y == 0 /*&& sign_z==0*/); 67 | 68 | episode_reward = 0f; 69 | SpawnGoal(); 70 | 71 | 72 | } 73 | 74 | public void UpdateGoalPosition(){ 75 | float MIN_RANGE = 1.5f; 76 | nb_step++; 77 | if(nb_step%dev_freq ==0){ 78 | sign_x = Random.Range(-1,2); 79 | sign_y = Random.Range(-1,2); 80 | } 81 | Vector3 new_pos_x = goal.transform.localPosition + new Vector3(goal_speed * sign_x,0f,0f); 82 | Vector3 new_pos_y = goal.transform.localPosition + new Vector3(0f,goal_speed*sign_y,0f); 83 | 84 | 85 | 86 | float distance_x = Vector3.Distance(new_pos_x,arm_base.transform.localPosition); 87 | float distance_y = Vector3.Distance(new_pos_y,arm_base.transform.localPosition); 88 | 89 | 90 | // Condition limite 91 | if( distance_x > ARM_RANGE - 0.1f || distance_x < MIN_RANGE ){ 92 | sign_x = -sign_x; 93 | } 94 | if( distance_y > ARM_RANGE - 0.1f || arm_base.transform.localPosition.y -0.25f > new_pos_y.y || distance_y < MIN_RANGE ){ 95 | sign_y = -sign_y; 96 | } 97 | Vector3 increment = new Vector3(goal_speed * sign_x,goal_speed*sign_y,0f); 98 | goal.transform.localPosition = increment + goal.transform.localPosition; 99 | 100 | } 101 | 102 | public void SpawnGoal(){ 103 | m_Goal_theta = Random.Range(0, Mathf.PI/ 2f); 104 | m_Goal_phi = Random.Range(0, Mathf.PI); 105 | m_Goal_rho = Random.Range(2,ARM_RANGE); 106 | 107 | 108 | // Sphérique 109 | var goalX = m_Goal_rho * Mathf.Sin(m_Goal_theta); 110 | var goalY = m_Goal_rho * Mathf.Cos(m_Goal_theta); 111 | 112 | 113 | goal.transform.localPosition = new Vector3(goalX, goalY, 0f); 114 | } 115 | 116 | // We collect the normalized rotations, angularal velocities, and velocities of both 117 | // limbs of the reacher as well as the relative position of the target and hand. 118 | public override void CollectObservations(VectorSensor sensor) 119 | { 120 | var angle_S1 = segment1.transform.rotation.eulerAngles.z; 121 | var angle_S2 = segment2.transform.rotation.eulerAngles.z; 122 | if(segment1.transform.rotation.eulerAngles.z > 120f){ 123 | angle_S1 = segment1.transform.rotation.eulerAngles.z - 360f; 124 | } 125 | if(segment2.transform.rotation.eulerAngles.z > 190f){ 126 | angle_S2 = segment2.transform.rotation.eulerAngles.z - 360f; 127 | angle_S2 = angle_S2 - angle_S1; 128 | } 129 | 130 | 131 | sensor.AddObservation(angle_S1); //1 132 | sensor.AddObservation(rb1.angularVelocity); //3 133 | sensor.AddObservation(rb1.velocity); //3 134 | 135 | sensor.AddObservation(angle_S2); //1 136 | sensor.AddObservation(rb2.angularVelocity); //3 137 | sensor.AddObservation(rb2.velocity); //3 138 | 139 | Vector3 goal_pos = goal.transform.position - ORIGIN; 140 | Vector3 hand_pos = hand.transform.position - ORIGIN; 141 | sensor.AddObservation(goal_pos); //3 142 | sensor.AddObservation(hand_pos); //3 143 | 144 | 145 | } 146 | 147 | // The agent's four actions correspond to torques on each of the two joints. 148 | public override void OnActionReceived(float[] vectorAction) 149 | { 150 | float reward_obtained; 151 | //config_joint.angularZMotion = ConfigurableJointMotion.Free; 152 | 153 | 154 | // Rotation segment 2 155 | var torque = Mathf.Clamp(vectorAction[1], -1f, 1f) * 10f; 156 | Debug.Log("torque s2: "+ vectorAction[1]); 157 | if(torque == 0){ 158 | rb2.constraints |= RigidbodyConstraints.FreezeRotationZ; 159 | //config_joint.angularZMotion = ConfigurableJointMotion.Locked; 160 | } 161 | else { 162 | rb2.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 163 | //config_joint.angularZMotion = ConfigurableJointMotion.Free; 164 | } 165 | rb2.AddRelativeTorque(new Vector3(0f, 0f, torque)); 166 | 167 | 168 | // Rotation segment 1 169 | torque = Mathf.Clamp(vectorAction[0], -1f, 1f) * 10f; 170 | Debug.Log("torque s1: "+ vectorAction[0]); 171 | if(torque == 0){ 172 | rb1.constraints |= RigidbodyConstraints.FreezeRotationZ; 173 | } 174 | else { 175 | rb1.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 176 | rb2.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 177 | } 178 | rb1.AddRelativeTorque(new Vector3(0f, 0f, torque)); 179 | 180 | UpdateGoalPosition(); 181 | 182 | float distance_to_target = Vector3.Distance(hand.transform.position,goal.transform.position); 183 | 184 | if(distance_to_target < positive_range){ 185 | reward_obtained = 0.1f; 186 | } 187 | else { 188 | reward_obtained = -1.0f * distance_to_target; 189 | 190 | } 191 | SetReward(reward_obtained); 192 | 193 | } 194 | 195 | 196 | 197 | public override float[] Heuristic() 198 | { 199 | var action = new float[3]; 200 | action[0] = Input.GetAxis("Horizontal"); 201 | action[1] = Input.GetAxis("Vertical"); 202 | 203 | return action; 204 | } 205 | 206 | 207 | 208 | 209 | } 210 | -------------------------------------------------------------------------------- /Unity/scripts_unity/ReacherAgent3D.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using MLAgents; 3 | using MLAgents.Sensors; 4 | 5 | public class ReacherAgent3D : Agent 6 | { 7 | // Game Object 8 | public GameObject segment1; 9 | public GameObject segment2; 10 | public GameObject hand; 11 | public GameObject arm_base; 12 | public GameObject goal; 13 | 14 | 15 | // Variable d'entrainement 16 | public float goal_speed; // Vitesse de déplacement de la balle 17 | public int dev_freq; // Fréquence à laquelle la balle change de direction (en step de simu) 18 | public float positive_range; // distance hand-goal à partir de laquelle la récompense est >0 19 | public float alpha; // constante pour régler la reward positive 20 | 21 | 22 | 23 | private float SEGMENT1_LENGTH; // longueur du premier segment du bras 24 | private Vector3 ORIGIN; // position absolue de la base 25 | private static float ARM_RANGE = 4.5f; // Longueur total du bras + hand 26 | 27 | 28 | // Angle pour la position du goal 29 | private float m_Goal_phi; 30 | private float m_Goal_theta; 31 | // rayon de la sphère sur lequel se trouve le goal 32 | private float m_Goal_rho; 33 | 34 | // Signe pour le déplacement du goal 35 | private int sign_x; 36 | private int sign_y; 37 | private int sign_z; 38 | 39 | private float episode_reward; 40 | private int nb_step; 41 | 42 | 43 | 44 | 45 | public override void Initialize() 46 | { 47 | episode_reward = 0f; 48 | SEGMENT1_LENGTH = segment2.transform.position.y; 49 | ORIGIN = arm_base.transform.position; 50 | 51 | } 52 | 53 | 54 | 55 | 56 | 57 | public override void OnEpisodeBegin() 58 | { 59 | 60 | nb_step = 0; 61 | // Place la sphère à atteindre à une position 62 | // Calcul fait en coordonnées sphériques 63 | 64 | do{ 65 | sign_x = Random.Range(-1,2); 66 | sign_y = Random.Range(-1,2); 67 | sign_z = Random.Range(-1,2); 68 | }while(sign_x == 0 && sign_y == 0 && sign_z==0); 69 | 70 | episode_reward = 0f; 71 | SpawnGoal(); 72 | 73 | 74 | } 75 | 76 | public void UpdateGoalPosition(){ 77 | float MIN_RANGE = 1.5f; 78 | nb_step++; 79 | if(nb_step%dev_freq ==0){ 80 | // Change la direction de la balle fréquemment 81 | sign_x = Random.Range(-1,2); 82 | sign_y = Random.Range(-1,2); 83 | sign_z = Random.Range(-1,2); 84 | } 85 | 86 | // On check si bouger le goal ne le fait pas sortir des limites 87 | Vector3 new_pos_x = goal.transform.localPosition + new Vector3(goal_speed * sign_x,0f,0f); 88 | Vector3 new_pos_y = goal.transform.localPosition + new Vector3(0f,goal_speed*sign_y,0f); 89 | Vector3 new_pos_z = goal.transform.localPosition + new Vector3(0f,0f,goal_speed*sign_z); 90 | 91 | 92 | float distance_x = Vector3.Distance(new_pos_x,arm_base.transform.localPosition); 93 | float distance_y = Vector3.Distance(new_pos_y,arm_base.transform.localPosition); 94 | float distance_z = Vector3.Distance(new_pos_z,arm_base.transform.localPosition); 95 | 96 | // Condition limite 97 | if( distance_x > ARM_RANGE - 0.1f || distance_x < MIN_RANGE ){ 98 | sign_x = -sign_x; 99 | } 100 | if( distance_z > ARM_RANGE - 0.1f || distance_z < MIN_RANGE ){ 101 | sign_z = -sign_z; 102 | } 103 | if( distance_y > ARM_RANGE - 0.1f || arm_base.transform.localPosition.y -0.25f > new_pos_y.y || distance_y < MIN_RANGE ){ 104 | sign_y = -sign_y; 105 | } 106 | Vector3 increment = new Vector3(goal_speed * sign_x,goal_speed*sign_y,goal_speed*sign_z); 107 | goal.transform.localPosition = increment + goal.transform.localPosition; 108 | 109 | } 110 | 111 | public void SpawnGoal(){ 112 | m_Goal_theta = Random.Range(0, Mathf.PI/ 2f); 113 | m_Goal_phi = Random.Range(0, Mathf.PI); 114 | m_Goal_rho = Random.Range(2,ARM_RANGE); 115 | 116 | 117 | // Sphérique 118 | var goalX = m_Goal_rho * Mathf.Sin(m_Goal_theta) * Mathf.Cos(m_Goal_phi); 119 | var goalZ = m_Goal_rho * Mathf.Sin(m_Goal_theta) * Mathf.Sin(m_Goal_phi) ; 120 | var goalY = m_Goal_rho * Mathf.Cos(m_Goal_theta); 121 | 122 | 123 | goal.transform.localPosition = new Vector3(goalX, goalY, goalZ); 124 | } 125 | 126 | // We collect the normalized rotations, angularal velocities, and velocities of both 127 | // limbs of the reacher as well as the relative position of the target and hand. 128 | public override void CollectObservations(VectorSensor sensor) 129 | { 130 | 131 | 132 | sensor.AddObservation(segment1.transform.rotation); //4 133 | sensor.AddObservation(segment2.transform.rotation); //4 134 | sensor.AddObservation(arm_base.transform.rotation); //4 135 | 136 | Vector3 goal_pos = goal.transform.position - ORIGIN; // Ramène les coordonnées au centre de la scene 137 | Vector3 hand_pos = hand.transform.position - ORIGIN; // Ramène les coordonnées au centre de la scene 138 | sensor.AddObservation(goal_pos); //3 139 | sensor.AddObservation(hand_pos); //3 140 | 141 | } 142 | 143 | // The agent's four actions correspond to torques on each of the two joints. 144 | public override void OnActionReceived(float[] vectorAction) 145 | { 146 | float reward_obtained; 147 | 148 | // Rotation de la base 149 | float consigne_base = Mathf.Clamp(vectorAction[2], -1f, 1f) * 180; 150 | arm_base.transform.rotation = Quaternion.Euler(0f,consigne_base,0f); 151 | 152 | 153 | // Rotation du segment 1 154 | float consigne_1 = Mathf.Clamp(vectorAction[0], -1f, 1f) * 90; 155 | segment1.transform.rotation = Quaternion.Euler(0f,consigne_base,consigne_1); 156 | 157 | // Rotation du segment 2 158 | float consigne_2 = Mathf.Clamp(vectorAction[1], -1f, 1f) * 155 + consigne_1; 159 | segment2.transform.rotation = Quaternion.Euler(0f,consigne_base,consigne_2); 160 | consigne_1 = consigne_1 * Mathf.PI/180f; 161 | 162 | // Reposition du segment 2 163 | consigne_base = consigne_base * Mathf.PI/180f; 164 | float pos_x = - SEGMENT1_LENGTH * Mathf.Sin(consigne_1); 165 | float pos_y = SEGMENT1_LENGTH * Mathf.Cos(consigne_1); 166 | float pos_z = pos_x * Mathf.Sin(consigne_base); 167 | 168 | pos_x = pos_x * Mathf.Cos(consigne_base); 169 | segment2.transform.localPosition = new Vector3(pos_x,pos_y + 0.5f,- pos_z); 170 | 171 | UpdateGoalPosition(); 172 | 173 | float distance_to_target = Vector3.Distance(hand.transform.position,goal.transform.position); 174 | 175 | if(distance_to_target < positive_range){ 176 | reward_obtained = 1/(distance_to_target * alpha); 177 | Debug.Log("Bingo !"); 178 | } 179 | else { 180 | reward_obtained = -1.0f * distance_to_target; 181 | 182 | } 183 | SetReward(reward_obtained); 184 | 185 | } 186 | 187 | 188 | 189 | public override float[] Heuristic() 190 | { 191 | var action = new float[3]; 192 | action[0] = Input.GetAxis("Horizontal"); 193 | action[1] = Input.GetAxis("Vertical"); 194 | action[2] = Input.GetAxis("Rotation"); 195 | 196 | return action; 197 | } 198 | 199 | 200 | 201 | 202 | } 203 | -------------------------------------------------------------------------------- /Unity/scripts_unity/ReacherAgent3DRb_v1.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using MLAgents; 3 | using MLAgents.Sensors; 4 | 5 | // TODO : convertir consigne en angle pour l'observation 6 | 7 | public class ReacherAgent : Agent 8 | { 9 | public GameObject segment1; 10 | public GameObject segment2; 11 | public GameObject hand; 12 | public GameObject arm_base; 13 | public GameObject joint; 14 | public GameObject goal; 15 | public float goal_speed; 16 | public int dev_freq; 17 | public float positive_range; 18 | 19 | private float grab_range = 0.88f; 20 | private float SEGMENT1_LENGTH; 21 | private Vector3 ORIGIN; 22 | private static float ARM_RANGE = 4.4f; 23 | 24 | private float m_Goal_theta; 25 | private float m_Goal_phi; 26 | private float m_Goal_rho; 27 | 28 | private int sign_x; 29 | private int sign_y; 30 | private int sign_z; 31 | private int nb_step; 32 | 33 | private Rigidbody rb1; 34 | private Rigidbody rb2; 35 | private Rigidbody rbb; 36 | //private Rigidbody rbg; 37 | //private Rigidbody rbh; 38 | 39 | // Collect the rigidbodies of the reacher in order to resue them for 40 | // observations and actions. 41 | public override void Initialize() 42 | { 43 | SEGMENT1_LENGTH = segment2.transform.position.y; 44 | ORIGIN = arm_base.transform.position; 45 | rb1 = segment1.GetComponent(); 46 | rb2 = segment2.GetComponent(); 47 | rbb = arm_base.GetComponent(); 48 | //rbg = goal.GetComponent(); 49 | //rbh = hand.GetComponent(); 50 | } 51 | 52 | 53 | 54 | 55 | // Resets the position of the agent and the goal. 56 | public override void OnEpisodeBegin() 57 | { 58 | 59 | nb_step = 0; 60 | // Place la sphère à atteindre à une position 61 | // Calcul fait en coordonnées sphériques 62 | 63 | do{ 64 | sign_x = Random.Range(-1,2); 65 | sign_y = Random.Range(-1,2); 66 | sign_z = Random.Range(-1,2); 67 | }while(sign_x == 0 && sign_y == 0 && sign_z==0); 68 | 69 | SpawnGoal(); 70 | 71 | 72 | } 73 | 74 | public void UpdateGoalPosition(){ 75 | float MIN_RANGE = 1.5f; 76 | nb_step++; 77 | if(nb_step%dev_freq ==0){ 78 | sign_x = Random.Range(-1,2); 79 | sign_y = Random.Range(-1,2); 80 | sign_z = Random.Range(-1,2); 81 | } 82 | Vector3 new_pos_x = goal.transform.localPosition + new Vector3(goal_speed * sign_x,0f,0f); 83 | Vector3 new_pos_y = goal.transform.localPosition + new Vector3(0f,goal_speed*sign_y,0f); 84 | Vector3 new_pos_z = goal.transform.localPosition + new Vector3(0f,0f,goal_speed*sign_z); 85 | 86 | 87 | float distance_x = Vector3.Distance(new_pos_x,arm_base.transform.localPosition); 88 | float distance_y = Vector3.Distance(new_pos_y,arm_base.transform.localPosition); 89 | float distance_z = Vector3.Distance(new_pos_z,arm_base.transform.localPosition); 90 | 91 | // Condition limite 92 | if( distance_x > ARM_RANGE - 0.1f || distance_x < MIN_RANGE ){ 93 | sign_x = -sign_x; 94 | } 95 | if( distance_z > ARM_RANGE - 0.1f || distance_z < MIN_RANGE ){ 96 | sign_z = -sign_z; 97 | } 98 | if( distance_y > ARM_RANGE - 0.1f || arm_base.transform.localPosition.y -0.25f > new_pos_y.y || distance_y < MIN_RANGE ){ 99 | sign_y = -sign_y; 100 | } 101 | Vector3 increment = new Vector3(goal_speed * sign_x,goal_speed*sign_y,goal_speed * sign_z); 102 | goal.transform.localPosition = increment + goal.transform.localPosition; 103 | 104 | } 105 | 106 | public void SpawnGoal(){ 107 | m_Goal_theta = Random.Range(0, Mathf.PI/ 2f); 108 | m_Goal_phi = Random.Range(0, Mathf.PI); 109 | m_Goal_rho = Random.Range(2,ARM_RANGE); 110 | 111 | 112 | // Sphérique 113 | var goalX = m_Goal_rho * Mathf.Sin(m_Goal_theta) * Mathf.Cos(m_Goal_phi); 114 | var goalZ = m_Goal_rho * Mathf.Sin(m_Goal_theta) * Mathf.Sin(m_Goal_phi) ; 115 | var goalY = m_Goal_rho * Mathf.Cos(m_Goal_theta); 116 | 117 | 118 | goal.transform.localPosition = new Vector3(goalX, goalY, goalZ); 119 | } 120 | 121 | // We collect the normalized rotations, angularal velocities, and velocities of both 122 | // limbs of the reacher as well as the relative position of the target and hand. 123 | public override void CollectObservations(VectorSensor sensor) 124 | { 125 | var angle_S1 = segment1.transform.rotation.eulerAngles.z; 126 | var angle_S2 = segment2.transform.rotation.eulerAngles.z; 127 | if(segment1.transform.rotation.eulerAngles.z > 120f){ 128 | angle_S1 = segment1.transform.rotation.eulerAngles.z - 360f; 129 | } 130 | if(segment2.transform.rotation.eulerAngles.z > 190f){ 131 | angle_S2 = segment2.transform.rotation.eulerAngles.z - 360f; 132 | } 133 | 134 | angle_S2 = angle_S2 - angle_S1; 135 | 136 | 137 | Vector3 goal_pos = goal.transform.position - ORIGIN; 138 | Vector3 hand_pos = hand.transform.position - ORIGIN; 139 | 140 | 141 | sensor.AddObservation(angle_S1/85.0f); //1 142 | sensor.AddObservation(rb1.angularVelocity/5.2f); //3 143 | sensor.AddObservation(rb1.velocity/5.2f); //3 144 | 145 | sensor.AddObservation(angle_S2/130.0f); //1 146 | sensor.AddObservation(rb2.angularVelocity/7.4f); //3 147 | sensor.AddObservation(rb2.velocity/7.4f); //3 148 | 149 | 150 | sensor.AddObservation(arm_base.transform.rotation.eulerAngles.y/360f); //1 151 | sensor.AddObservation(rbb.angularVelocity.y/3.19f); //1 152 | 153 | 154 | sensor.AddObservation(goal_pos/ARM_RANGE); //3 155 | sensor.AddObservation(hand_pos/ARM_RANGE); //3 156 | 157 | 158 | } 159 | 160 | // The agent's four actions correspond to torques on each of the two joints. 161 | public override void OnActionReceived(float[] vectorAction) 162 | { 163 | float reward_obtained; 164 | 165 | 166 | // Rotation segment 2 167 | var torque = Mathf.Clamp(vectorAction[1], -1f, 1f) * 10f; 168 | if(torque == 0){ 169 | rb2.constraints |= RigidbodyConstraints.FreezeRotationZ; 170 | } 171 | else { 172 | rb2.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 173 | 174 | } 175 | rb2.AddRelativeTorque(new Vector3(0f, 0f, torque)); 176 | 177 | 178 | // Rotation segment 1 179 | torque = Mathf.Clamp(vectorAction[0], -1f, 1f) * 10f; 180 | if(torque == 0){ 181 | rb1.constraints |= RigidbodyConstraints.FreezeRotationZ; 182 | } 183 | else { 184 | rb1.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 185 | rb2.constraints &= ~ RigidbodyConstraints.FreezeRotationZ; 186 | } 187 | rb1.AddRelativeTorque(new Vector3(0f, 0f, torque)); 188 | 189 | 190 | // Rotation base 191 | torque = Mathf.Clamp(vectorAction[2], -1f, 1f); 192 | if(torque == 0){ 193 | rbb.constraints |= RigidbodyConstraints.FreezeRotationY; 194 | } 195 | else { 196 | rbb.constraints &= ~ RigidbodyConstraints.FreezeRotationY; 197 | } 198 | rbb.AddRelativeTorque(new Vector3(0f,torque,0f)); 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | UpdateGoalPosition(); 207 | 208 | float distance_to_target = Vector3.Distance(hand.transform.position,goal.transform.position); 209 | //Debug.Log("ddistance_to_target : "+ distance_to_target); 210 | if(distance_to_target < positive_range){ 211 | reward_obtained = 0.1f; 212 | } 213 | else { 214 | reward_obtained = -1.0f * distance_to_target; 215 | 216 | } 217 | SetReward(reward_obtained); 218 | 219 | //if(distance_to_target <= grab_range && goal.GetComponent() == null){ 220 | // Debug.Log("creating joint : "); 221 | // FixedJoint grabbing = goal.AddComponent(); 222 | // grabbing.connectedBody = rbh; 223 | //} 224 | 225 | } 226 | 227 | 228 | 229 | public override float[] Heuristic() 230 | { 231 | var action = new float[3]; 232 | action[0] = Input.GetAxis("Horizontal"); 233 | action[1] = Input.GetAxis("Vertical"); 234 | action[2] = Input.GetAxis("Rotation"); 235 | 236 | return action; 237 | } 238 | 239 | 240 | 241 | 242 | } 243 | -------------------------------------------------------------------------------- /Unity/unity_package/ReacherAgent.unitypackage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/unity_package/ReacherAgent.unitypackage -------------------------------------------------------------------------------- /Unity/unity_package/ReacherAgentRb2D_v1.unitypackage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/unity_package/ReacherAgentRb2D_v1.unitypackage -------------------------------------------------------------------------------- /Unity/unity_package/ReacherAgentRb2D_v2.unitypackage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/unity_package/ReacherAgentRb2D_v2.unitypackage -------------------------------------------------------------------------------- /Unity/unity_package/ReacherAgentRb3D_v1.unitypackage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/unity_package/ReacherAgentRb3D_v1.unitypackage -------------------------------------------------------------------------------- /Unity/unity_package/braccio_package.unitypackage: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/Unity/unity_package/braccio_package.unitypackage -------------------------------------------------------------------------------- /WebCam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/WebCam.png -------------------------------------------------------------------------------- /braccio_reacher.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/braccio_reacher.gif -------------------------------------------------------------------------------- /braccio_unity.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/braccio_unity.gif -------------------------------------------------------------------------------- /opencv/ball_tracking.py: -------------------------------------------------------------------------------- 1 | # import the necessary packages 2 | from collections import deque 3 | import numpy as np 4 | import cv2 5 | import time 6 | 7 | 8 | # define the lower and upper boundaries of the "orange" 9 | # ball in the HSV color space, then initialize the 10 | # list of tracked points 11 | orangeLower = (0,122,150) 12 | orangeUpper = (25,194,246) 13 | 14 | cam = cv2.VideoCapture(0) 15 | cam.set(cv2.CAP_PROP_FRAME_WIDTH,640) 16 | cam.set(cv2.CAP_PROP_FRAME_HEIGHT,380) 17 | # allow the camera or video file to warm up 18 | time.sleep(2.0) 19 | 20 | # keep looping 21 | while True: 22 | # grab the current frame 23 | ret,frame = cam.read() 24 | # resize the frame, blur it, and convert it to the HSV 25 | # color space 26 | #frame = cv2.resize(frame, width=600) 27 | blurred = cv2.GaussianBlur(frame, (11, 11), 0) 28 | hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV) 29 | # construct a mask for the color "orange", then perform 30 | # a series of dilations and erosions to remove any small 31 | # blobs left in the mask 32 | mask = cv2.inRange(hsv, orangeLower, orangeUpper) 33 | mask = cv2.erode(mask, None, iterations=2) 34 | mask = cv2.dilate(mask, None, iterations=2) 35 | _,contours,_ = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) 36 | # only proceed if at least one contour was found 37 | if len(contours) > 0: 38 | # find the largest contour in the mask, then use 39 | # it to compute the minimum enclosing circle and 40 | # centroid 41 | c = max(contours, key=cv2.contourArea) 42 | ((x, y), radius) = cv2.minEnclosingCircle(c) 43 | M = cv2.moments(c) 44 | center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"])) 45 | # only proceed if the radius meets a minimum size 46 | if radius > 10: 47 | # draw the circle and centroid on the frame, 48 | # then update the list of tracked points 49 | cv2.circle(frame, (int(x), int(y)), int(radius),(0, 255, 255), 2) 50 | cv2.circle(frame, center, 5, (0, 0, 255), -1) 51 | 52 | # show the frame to our screen 53 | cv2.imshow("Frame", frame) 54 | key = cv2.waitKey(1) & 0xFF 55 | # if the 'q' key is pressed, stop the loop 56 | if key == ord("q"): 57 | break 58 | 59 | cam.release() 60 | cv2.destroyAllWindows() 61 | 62 | 63 | -------------------------------------------------------------------------------- /opencv/balle_braccio.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/balle_braccio.jpg -------------------------------------------------------------------------------- /opencv/balle_orange.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/balle_orange.jpg -------------------------------------------------------------------------------- /opencv/balle_orange_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/balle_orange_2.jpg -------------------------------------------------------------------------------- /opencv/camera/cam_calibration.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import glob 4 | 5 | # termination criteria 6 | criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) 7 | 8 | # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) 9 | objp = np.zeros((6*7,3), np.float32) 10 | objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2) 11 | 12 | # Arrays to store object points and image points from all the images. 13 | objpoints = [] # 3d point in real world space 14 | imgpoints = [] # 2d points in image plane. 15 | 16 | images = glob.glob('*.jpg') 17 | 18 | for fname in images: 19 | print(fname) 20 | img = cv2.imread(fname) 21 | gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) 22 | 23 | # Find the chess board corners 24 | ret, corners = cv2.findChessboardCorners(gray, (7,6),None) 25 | 26 | # If found, add object points, image points (after refining them) 27 | if ret == True: 28 | objpoints.append(objp) 29 | 30 | corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) 31 | imgpoints.append(corners2) 32 | 33 | # Draw and display the corners 34 | img = cv2.drawChessboardCorners(img, (7,6), corners2,ret) 35 | cv2.imshow('img',img) 36 | cv2.waitKey(500) 37 | 38 | 39 | ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None) 40 | np.savetxt("cam_mtx.txt",mtx) 41 | np.savetxt("disorsion_param.txt",dist) 42 | print(ret) 43 | 44 | cv2.destroyAllWindows() 45 | 46 | 47 | -------------------------------------------------------------------------------- /opencv/camera/cam_mtx.txt: -------------------------------------------------------------------------------- 1 | 627.1920161485502376,0.000000000000000000,343.2517088903035187 2 | 0.000000000000000000,593.5871772591797253,235.8795884782900600 3 | 0.000000000000000000,0.000000000000000000,1.000000000000000000 4 | -------------------------------------------------------------------------------- /opencv/camera/disorsion_param.txt: -------------------------------------------------------------------------------- 1 | -3.680191370263419248e-02,9.987614108149600600e-01,4.996557377198188044e-02,3.579539571645915658e-02,-2.066593050264101450e+00 2 | -------------------------------------------------------------------------------- /opencv/camera/scene00001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00001.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00051.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00051.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00101.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00101.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00151.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00151.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00201.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00201.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00251.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00251.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00301.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00301.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00351.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00351.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00401.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00401.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00451.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00451.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00501.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00501.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00601.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00601.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00651.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00651.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00701.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00701.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00751.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00751.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00801.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00801.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00851.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00851.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00901.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00901.jpg -------------------------------------------------------------------------------- /opencv/camera/scene00951.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene00951.jpg -------------------------------------------------------------------------------- /opencv/camera/scene01001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene01001.jpg -------------------------------------------------------------------------------- /opencv/camera/scene01051.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene01051.jpg -------------------------------------------------------------------------------- /opencv/camera/scene01101.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/camera/scene01101.jpg -------------------------------------------------------------------------------- /opencv/color_boundaries.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | 4 | image_hsv = None # global ;( 5 | pixel = (20,60,80) # some stupid default 6 | 7 | # mouse callback function 8 | def pick_color(event,x,y,flags,param): 9 | if event == cv2.EVENT_LBUTTONDOWN: 10 | pixel = image_hsv[y,x] 11 | 12 | #you might want to adjust the ranges(+-10, etc): 13 | upper = np.array([pixel[0] + 10, pixel[1] + 10, pixel[2] + 30]) 14 | lower = np.array([pixel[0] - 10, pixel[1] - 10, pixel[2] - 30]) 15 | print(pixel, lower, upper) 16 | 17 | image_mask = cv2.inRange(image_hsv,lower,upper) 18 | cv2.imshow("mask",image_mask) 19 | 20 | def main(): 21 | import sys 22 | global image_hsv, pixel # so we can use it in mouse callback 23 | 24 | image_src = cv2.imread(sys.argv[1]) # pick.py my.png 25 | if image_src is None: 26 | print ("the image read is None............") 27 | return 28 | cv2.imshow("bgr",image_src) 29 | 30 | ## NEW ## 31 | cv2.namedWindow('hsv') 32 | cv2.setMouseCallback('hsv', pick_color) 33 | 34 | # now click into the hsv img , and look at values: 35 | image_hsv = cv2.cvtColor(image_src,cv2.COLOR_BGR2HSV) 36 | cv2.imshow("hsv",image_hsv) 37 | 38 | cv2.waitKey(0) 39 | cv2.destroyAllWindows() 40 | 41 | if __name__=='__main__': 42 | main() 43 | -------------------------------------------------------------------------------- /opencv/test_marker.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/opencv/test_marker.jpg -------------------------------------------------------------------------------- /opencv/test_marker.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import cv2.aruco as aruco 4 | import sys, time, math 5 | 6 | # -- Id to find in the image 7 | id_to_find = 6 8 | marker_size = 2.45 # cm 9 | 10 | # Constant parameters used in Aruco methods 11 | ARUCO_PARAMETERS = aruco.DetectorParameters_create() 12 | ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL) 13 | 14 | # Get the camera calibration parameters 15 | path = "../camera/" 16 | camera_matrix = np.loadtxt(path+"cam_mtx.txt",delimiter=",") 17 | camera_distorsion = np.loadtxt(path+"disorsion_param.txt",delimiter=",") 18 | 19 | print(camera_matrix) 20 | print(camera_distorsion) 21 | 22 | # flip 180 degrees matrix around the x axis 23 | R_flip = np.zeros((3,3),dtype=np.float32) 24 | R_flip[0,0] = 1.0 25 | R_flip[1,1] = -1.0 26 | R_flip[2,2] = -1.0 27 | 28 | # get image from webcam 29 | cam = cv2.VideoCapture(0) 30 | cam.set(cv2.CAP_PROP_FRAME_WIDTH,640) 31 | cam.set(cv2.CAP_PROP_FRAME_HEIGHT,380) 32 | 33 | while True: 34 | 35 | # Read the camera frame 36 | # Ret est un boolean qui nous informe pour savoi rsi 37 | ret, frame = cam.read() 38 | 39 | # Convert in gray scale 40 | gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 41 | 42 | # Find marker in the image 43 | corners,ids, _ = aruco.detectMarkers(image=gray, dictionary=ARUCO_DICT, parameters =ARUCO_PARAMETERS, cameraMatrix = camera_matrix, distCoeff = camera_distorsion) 44 | 45 | if ids is not None and ids[0] == id_to_find : 46 | 47 | ret = aruco.estimatePoseSingleMarkers (corners,marker_size, camera_matrix,camera_distorsion) 48 | rot_vec,trans_vec = ret[0][0,0,:], ret[1][0,0,:] 49 | aruco.drawDetectedMarkers(frame,corners) 50 | aruco.drawAxis(frame,camera_matrix,camera_distorsion,rot_vec,trans_vec,10) 51 | str_position = "MARKER Position x=%4.0f y=%4.0f z=%4.0f"%(trans_vec[0], trans_vec[1], trans_vec[2]) 52 | print(str_position) 53 | #cv2.putText(frame,str_position, (0,100), font,1,(0,255),2,cv2.LINE_AA) 54 | # Display the frame 55 | cv2.imshow('frame',frame) 56 | 57 | # use q to quit 58 | key = cv2.waitKey(1) & 0xFF 59 | 60 | if key == ord('q'): 61 | cam.release() 62 | cv2.destroyAllWindows() 63 | break 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /opencv/test_seuillage.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import cv2 as cv 3 | 4 | img = cv.imread("balle_orange_2.jpg") 5 | hsv = cv.cvtColor(img,cv.COLOR_BGR2HSV) 6 | mask = cv.inRange(hsv,(0,122,150),(25,194,246)) 7 | cv.imshow("orange",mask) 8 | cv.waitKey() 9 | cv.destroyAllWindows() 10 | -------------------------------------------------------------------------------- /reacher_rigidBodies.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sabeaussan/ROS_Unity/a963975e7c4d63aca8adb6825a992c7c15cfab28/reacher_rigidBodies.gif --------------------------------------------------------------------------------