├── nao_gazebo ├── CMakeLists.txt └── package.xml ├── gazebo_naoqi_control ├── models │ ├── model.xacro │ └── nao.xacro ├── CMakeLists.txt ├── launch │ ├── nao_gazebo.launch │ └── display.launch ├── package.xml ├── config │ ├── gazebo_ros_control_params.yaml │ └── urdf.rviz ├── cmake │ └── FindNAOqiSIM.cmake ├── include │ └── gazebo_naoqi_control │ │ └── gazebo_naoqi_control_plugin.h └── src │ └── gazebo_naoqi_control_plugin.cpp ├── LICENSE.txt └── README.md /nao_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nao_gazebo) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/models/model.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gazebo_naoqi_control) 3 | 4 | 5 | # Tell CMake where to find "FindNAOqiSIM.cmake" 6 | list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake") 7 | 8 | # Tell Program where to find NAOqi/NAOqiSIM bin 9 | add_definitions(-DNAOQI_SDK="$ENV{AL_DIR}") 10 | add_definitions(-DNAOQI_SIM_SDK="$ENV{AL_SIM_DIR}") 11 | 12 | # Load catkin and all dependencies required for this package 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | gazebo_ros 16 | control_toolbox 17 | ) 18 | 19 | # Load NAOqi dependencies 20 | find_package(NAOqiSIM REQUIRED COMPONENTS 21 | alnaosim 22 | alsimutils 23 | alrobotmodel 24 | ) 25 | 26 | if(NAOqiSIM_FOUND) 27 | message(STATUS "NAOqi Simulator SDK found!") 28 | endif() 29 | 30 | # Depend on system install of Gazebo 31 | find_package(gazebo REQUIRED) 32 | find_package(Boost REQUIRED) 33 | 34 | link_directories(${GAZEBO_LIBRARY_DIRS}) 35 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${NAOqiSIM_INCLUDE_DIRS} include) 36 | 37 | add_library(gazebo_naoqi_control src/gazebo_naoqi_control_plugin.cpp) 38 | target_link_libraries(gazebo_naoqi_control ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${NAOqiSIM_LIBRARIES}) 39 | 40 | catkin_package( 41 | DEPENDS 42 | roscpp 43 | gazebo_ros 44 | control_toolbox 45 | ) 46 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Konstantinos Chatzilygeroudis 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 9 | in the documentation and/or other materials provided with the distribution. 10 | 11 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 12 | from this software without specific prior written permission. 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 15 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 16 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 17 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 18 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 19 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 20 | -------------------------------------------------------------------------------- /nao_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | nao_gazebo 23 | 0.0.1 24 | nao_gazebo metapackage contains packages to integrate into Gazebo Aldebaran's NAO robot via NAO Simulator C++ SDK. 25 | 26 | Konstantinos Chatzilygeroudis 27 | 28 | 29 | BSD 30 | 31 | https://github.com/costashatz/nao_gazebo 32 | https://github.com/costashatz/nao_gazebo/issues 33 | 34 | Konstantinos Chatzilygeroudis 35 | 36 | catkin 37 | 38 | gazebo_naoqi_control 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/launch/nao_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 21 | 22 | gazebo_naoqi_control 23 | 0.0.1 24 | Integration of NAO Simulator SDK in Gazebo 25 | 26 | Konstantinos Chatzilygeroudis 27 | 28 | BSD 29 | 30 | https://github.com/costashatz/nao_gazebo 31 | https://github.com/costashatz/nao_gazebo/issues 32 | 33 | Konstantinos Chatzilygeroudis 34 | 35 | catkin 36 | 37 | 38 | gazebo_ros 39 | roscpp 40 | control_toolbox 41 | 42 | 43 | gazebo_ros 44 | roscpp 45 | control_toolbox 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/config/gazebo_ros_control_params.yaml: -------------------------------------------------------------------------------- 1 | nao_dcm/gazebo_ros_control/pid_gains: 2 | HeadYaw: {p: 1000.0, i: 100.0, d: 1.0} 3 | 4 | HeadPitch: {p: 1000.0, i: 100.0, d: 1.0} 5 | 6 | LShoulderPitch: {p: 80.0, i: 4.0, d: 2.0} 7 | 8 | LElbowYaw: {p: 110.0, i: 0.0, d: 0.0} 9 | 10 | LElbowRoll: {p: 30.0, i: 0.7, d: 0.3} 11 | 12 | LShoulderRoll: {p: 50.0, i: 5.0, d: 0.7} 13 | 14 | LWristYaw: {p: 1.0, i: 0.13, d: 0.01} 15 | 16 | LHand: {p: 100.0, i: 100.0, d: 3.0} 17 | 18 | RShoulderPitch: {p: 80.0, i: 4.0, d: 2.0} 19 | 20 | RElbowYaw: {p: 110.0, i: 0.0, d: 0.0} 21 | 22 | RElbowRoll: {p: 30.0, i: 0.7, d: 0.3} 23 | 24 | RShoulderRoll: {p: 50.0, i: 5.0, d: 0.7} 25 | 26 | RWristYaw: {p: 1.0, i: 0.13, d: 0.01} 27 | 28 | RHand: {p: 100.0, i: 100.0, d: 3.0} 29 | 30 | LHipYawPitch: {p: 500.0, i: 0.0, d: 0.1} 31 | 32 | LHipRoll: {p: 500.0, i: 0.0, d: 0.1} 33 | 34 | LHipPitch: {p: 500.0, i: 0.0, d: 0.1} 35 | 36 | LKneePitch: {p: 500.0, i: 0.0, d: 0.1} 37 | 38 | LAnklePitch: {p: 500.0, i: 0.0, d: 0.1} 39 | 40 | LAnkleRoll: {p: 500.0, i: 0.0, d: 0.1} 41 | 42 | RHipYawPitch: {p: 500.0, i: 0.0, d: 0.1} 43 | 44 | RHipRoll: {p: 500.0, i: 0.0, d: 0.1} 45 | 46 | RHipPitch: {p: 500.0, i: 0.0, d: 0.1} 47 | 48 | RKneePitch: {p: 500.0, i: 0.0, d: 0.1} 49 | 50 | RAnklePitch: {p: 500.0, i: 0.0, d: 0.1} 51 | 52 | RAnkleRoll: {p: 500.0, i: 0.0, d: 0.1} 53 | # nao_dcm/gazebo_ros_control/pid_gains: 54 | # HeadYaw: {p: 100.0, i: 0.0, d: 0.0} 55 | 56 | # HeadPitch: {p: 100.0, i: 0.0, d: 0.0} 57 | 58 | # LShoulderPitch: {p: 1200.0, i: 4.0, d: 40.0} 59 | 60 | # LShoulderRoll: {p: 100.0, i: 0.0, d: 3.0} 61 | 62 | # LElbowYaw: {p: 1000.0, i: 0.0, d: 0.0} 63 | 64 | # LElbowRoll: {p: 100.0, i: 0.0, d: 0.0} 65 | 66 | # LWristYaw: {p: 100.0, i: 0.0, d: 0.0} 67 | 68 | # LHand: {p: 100.0, i: 0.0, d: 0.0} 69 | 70 | # RShoulderPitch: {p: 1200.0, i: 4.0, d: 40.0} 71 | 72 | # RShoulderRoll: {p: 100.0, i: 0.0, d: 3.0} 73 | 74 | # RElbowYaw: {p: 1000.0, i: 0.0, d: 0.0} 75 | 76 | # RElbowRoll: {p: 100.0, i: 0.0, d: 0.0} 77 | 78 | # RWristYaw: {p: 100.0, i: 0.0, d: 0.0} 79 | 80 | # RHand: {p: 100.0, i: 0.0, d: 0.0} 81 | 82 | # LHipYawPitch: {p: 500.0, i: 0.0, d: 0.1} 83 | 84 | # LHipRoll: {p: 1000.0, i: 0.0, d: 5.0} 85 | 86 | # LHipPitch: {p: 1000.0, i: 0.0, d: 5.0} 87 | 88 | # LKneePitch: {p: 500.0, i: 0.0, d: 0.1} 89 | 90 | # LAnklePitch: {p: 500.0, i: 0.0, d: 0.1} 91 | 92 | # LAnkleRoll: {p: 500.0, i: 0.0, d: 0.1} 93 | 94 | # RHipYawPitch: {p: 500.0, i: 0.0, d: 0.1} 95 | 96 | # RHipRoll: {p: 1000.0, i: 0.0, d: 5.0} 97 | 98 | # RHipPitch: {p: 1000.0, i: 0.0, d: 5.0} 99 | 100 | # RKneePitch: {p: 500.0, i: 0.0, d: 0.1} 101 | 102 | # RAnklePitch: {p: 500.0, i: 0.0, d: 0.1} 103 | 104 | # RAnkleRoll: {p: 500.0, i: 0.0, d: 0.1} 105 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/cmake/FindNAOqiSIM.cmake: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | #These are NAOqi SIM's known components (ie. libraries) 4 | set(NAOqi_COMPONENTS 5 | alaudio 6 | albonjourdiscovery 7 | alextractor 8 | allog 9 | almodelutils 10 | alproject 11 | alresource 12 | altools 13 | alautomatictest 14 | alboxrary 15 | alfile 16 | almathinternal 17 | almotion 18 | alpythonbridge 19 | alserial 20 | altts 21 | albehaviorinfo 22 | alcommon 23 | allauncher 24 | almath 25 | almotionrecorder 26 | alpythontools 27 | alsoap 28 | alvalue 29 | albehavior 30 | alerror 31 | allogremote 32 | almemoryfastaccess 33 | alparammanager 34 | alremotecall 35 | althread 36 | alvision 37 | alproxies 38 | qi 39 | qitype 40 | alnaosim 41 | alsimutils 42 | alrobotmodel) 43 | 44 | 45 | #Set INCLUDE hints 46 | set(NAOqi_INCLUDE_HINTS 47 | "${NAOqi_SIM_DIR}/include" 48 | "$ENV{AL_SIM_DIR}/include" ) 49 | 50 | # Set LIBRARY hints 51 | set(NAOqi_LIBRARY_HINTS 52 | "${NAOqi_SIM_DIR}/lib" 53 | "$ENV{AL_SIM_DIR}/lib" ) 54 | 55 | # Find include directories 56 | find_path(NAOqiSIM_INCLUDE_DIR alcommon/alproxy.h HINTS ${NAOqi_INCLUDE_HINTS} ) 57 | 58 | # Verify we know about all the components requested 59 | # and remove those we don't know about 60 | set(NAOqi_FILTERED_COMPONENTS ${NAOqiSIM_FIND_COMPONENTS}) 61 | 62 | if ( NAOqiSIM_FIND_COMPONENTS ) 63 | foreach(comp ${NAOqiSIM_FIND_COMPONENTS}) 64 | list(FIND NAOqi_COMPONENTS ${comp} ${comp}_KNOWN) 65 | if (${comp}_KNOWN EQUAL -1) 66 | list(REMOVE_ITEM NAOqi_FILTERED_COMPONENTS ${comp}) 67 | message(STATUS "Unknown NAOqi component ${comp}") 68 | endif() 69 | endforeach() 70 | endif() 71 | 72 | list(LENGTH NAOqi_FILTERED_COMPONENTS NAOqiSIM_NUMBER_OF_COMPONENTS) 73 | set(NAOqiSIM_FOUND_COMPONENTS TRUE) 74 | 75 | # Look for components (ie. libraries) 76 | if( ${NAOqiSIM_NUMBER_OF_COMPONENTS} ) 77 | foreach(comp ${NAOqi_FILTERED_COMPONENTS}) 78 | #Look for the actual library here 79 | find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${NAOqi_LIBRARY_HINTS}) 80 | if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND) 81 | message(STATUS "Could not find NAOqi's ${comp}") 82 | set(NAOqiSIM_FOUND_COMPONENTS FALSE) 83 | else() 84 | #If everything went well append this component to list of libraries 85 | list(APPEND NAOqiSIM_LIBRARY ${${comp}_LIBRARY}) 86 | endif() 87 | endforeach() 88 | else() 89 | message(STATUS "No NAOqi components specified") 90 | endif() 91 | 92 | 93 | # Handle the QUIET and REQUIRED arguments 94 | include(FindPackageHandleStandardArgs) 95 | find_package_handle_standard_args( 96 | NAOqiSIM #Package name 97 | DEFAULT_MSG 98 | # Variables required to evaluate as TRUE 99 | NAOqiSIM_LIBRARY 100 | NAOqiSIM_INCLUDE_DIR 101 | NAOqiSIM_FOUND_COMPONENTS) 102 | 103 | # Copy the values of the advanced variables to the user-facing ones 104 | set(NAOqiSIM_LIBRARIES ${NAOqiSIM_LIBRARY} ) 105 | set(NAOqiSIM_INCLUDE_DIRS ${NAOqiSIM_INCLUDE_DIR} ) 106 | set(NAOqiSIM_FOUND ${NAOQISIM_FOUND}) 107 | 108 | # If NAOqi was found, update NAOqi_DIR to show where it was found 109 | if ( NAOqiSIM_FOUND ) 110 | get_filename_component(NAOqi_NEW_DIR "${NAOqiSIM_INCLUDE_DIRS}/../" ABSOLUTE) 111 | endif() 112 | set(NAOqiSIM_DIR ${NAOqi_NEW_DIR} CACHE FILEPATH "NAOqi root directory" FORCE) 113 | 114 | #Hide these variables 115 | mark_as_advanced(NAOqiSIM_INCLUDE_DIR NAOqiSIM_LIBRARY NAOQISIM_FOUND) 116 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # nao_gazebo 2 | 3 | ## ROS Packages to simulate Aldebaran's NAO Humanoid in Gazebo through NAOqi Simulator SDK 4 | 5 | - Supports **all versions and body types of NAO**. 6 | - **Written purely in C++**. 7 | - **Standalone ROS package** 8 | 9 | 10 | Version 11 | ---- 12 | 13 | 0.0.1 (Beta) 14 | 15 | Requirements 16 | ----------- 17 | 18 | nao_gazebo requires several packages to be installed in order to work properly: 19 | 20 | * [ROS] - ROS >= **Hydro** 21 | * [ROS Control] - **Version >=0.6.0** [control_toolbox should be installed] 22 | * [Gazebo] - **Version 2.2.5** - *Should work on newer versions but haven't tested it* 23 | * [NAO meshes] - For visualization purposes 24 | 25 | Building/Compiling 26 | ------------------ 27 | 28 | You need to have either environmental variable **AL_SIM_DIR** or CMake option **NAOqi_SIM_DIR** pointing to the C++ NAOqi Simulator SDK path. 29 | 30 | Also, you need to have either environmental variable **AL_DIR** or CMake option **NAOqi_DIR** pointing to the C++ NAOqi SDK path. 31 | 32 | Check *cmake/FindNAOqiSIM.cmake* for more details. 33 | 34 | Basic Usage 35 | -------------- 36 | 37 | ### Bringup nao_gazebo 38 | ```sh 39 | roslaunch gazebo_naoqi_control nao_gazebo.launch 40 | ``` 41 | 42 | This will open up Gazebo with NAO model loaded and simulation paused! You need to open Choregraphe (or any other driver) to give commands to NAO as you would give to a real robot. 43 | 44 | **BE CAREFUL:** 45 | 46 | NAO at simulation start up is stiffened off. You need to connect to Choreographe and enable stiffness (wake up the robot), before the robot falls down. DO NOT enable stiffness with the simulation paused. It will cause NAOqi to be stalled and no further commands can be issued! 47 | 48 | ### Visualize modified model/urdf 49 | ```sh 50 | roslaunch gazebo_naoqi_control display.launch 51 | ``` 52 | 53 | This will open up RViz with NAO model loaded for debugging purposes. 54 | 55 | URDF Modifications 56 | ------------------ 57 | Please feel free to check *models/nao.xacro* file for modifying for example sensor properties... 58 | 59 | ### Plugin SDF elements 60 | * **robotNamespace** - provides the robot namespace 61 | * **controlPeriod** - control period for reading/writing sensor/actuator values 62 | * **port** - port to initialize NAOqi 63 | * **modelType** - type of model to load. Check the path in simulator SDK: *${SIMULATOR_SDK}/share/alrobotmodel/models/* 64 | 65 | ### Load plugin in URDF 66 | 67 | Classic gazebo model plugin insertion.. See example in **models/model.xacro**. In practice create a file like **model.xacro** and tune the xacro properties. 68 | 69 | ``` 70 | 71 | 72 | your_robot_namespace 73 | the_control_period_you_wish 74 | port_to_initialize_naoqi 75 | the_model_you_want 76 | 77 | 78 | ``` 79 | 80 | **NOTE that sensor names must be as they are in nao.xacro file..** 81 | 82 | Notes/Limitations 83 | ----------------- 84 | * Tutorials will become available as soon as possible. 85 | * Cameras, IMU, sonars and FSRs integration is completed, but in beta/testing stage. 86 | * **Fingers/Hand joints are not simulated yet..** 87 | * *Integration for LED, IR, Bumper, Tactile and Audio hardware is not available and is not on my agenda*. So, **feel free to contribute in that direction**. 88 | * I have tested the plugin for V40 and V50 robot models! Using it with older versions should work, but isn't tested! 89 | * **NAO now walks!!** Thanks to @jlanca .. 90 | 91 | Future Work 92 | ------------ 93 | * **Integrate Fingers/Hand joints.** 94 | * Investigate if we can bind simulated NAOqi's time with Gazebo's simulated time. 95 | * Enable stiffness by default when simulation is started. 96 | * Integration of more sensors. 97 | * **PID values need tuning!!!** 98 | * ..... 99 | 100 | License 101 | ---- 102 | 103 | BSD 104 | 105 | 106 | Copyright (c) 2014-2015, **Konstantinos Chatzilygeroudis** 107 | 108 | [ros]: http://www.ros.org 109 | [gazebo]: http://gazebosim.org 110 | [ros control]: http://wiki.ros.org/ros_control 111 | [nao meshes]: https://github.com/ros-naoqi/nao_meshes 112 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/include/gazebo_naoqi_control/gazebo_naoqi_control_plugin.h: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2015, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | #ifndef GAZEBO_NAOQI_CONTROL_GAZEBO_NAOQI_CONTROL_PLUGIN 24 | #define GAZEBO_NAOQI_CONTROL_GAZEBO_NAOQI_CONTROL_PLUGIN 25 | 26 | // ROS includes 27 | #include 28 | #include 29 | #include 30 | 31 | // Boost includes 32 | #include 33 | 34 | // Gazebo includes 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | // ros_control 42 | #include 43 | 44 | // NAOqi includes 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | // General Includes 51 | #include 52 | 53 | namespace gazebo 54 | { 55 | class GazeboNaoqiControlPlugin : public ModelPlugin 56 | { 57 | public: 58 | GazeboNaoqiControlPlugin(); 59 | ~GazeboNaoqiControlPlugin(); 60 | 61 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 62 | void Update(); 63 | 64 | private: 65 | void readSim(); 66 | void writeSim(ros::Time time, ros::Duration period); 67 | 68 | void initSensors(); 69 | 70 | void onCameraUpdate(const Sim::CameraSensor* _camera, const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format); 71 | 72 | void onImuUpdate(sensors::ImuSensorPtr _sensor); 73 | 74 | void onSonarUpdate(sensors::RaySensorPtr _gazebo_sonar, const Sim::SonarSensor* _sonar); 75 | 76 | void onFSRUpdate(sensors::ContactSensorPtr _gazebo_fsr, const Sim::FSRSensor* _fsr); 77 | 78 | protected: 79 | // NAOqi Parameters 80 | Sim::SimLauncher* naoqi_sim_launcher_; 81 | Sim::Model* naoqi_model_; 82 | Sim::HALInterface* naoqi_hal_; 83 | 84 | // NAOqi Joint Actuators/Sensors 85 | std::vector joints_; 86 | 87 | std::vector angle_sensors_; 88 | 89 | std::vector angle_actuators_; 90 | 91 | const Sim::CameraSensor* top_camera_; 92 | const Sim::CameraSensor* bottom_camera_; 93 | const Sim::InertialSensor* inertial_sensor_; 94 | const Sim::SonarSensor* left_sonar_; 95 | const Sim::SonarSensor* right_sonar_; 96 | const Sim::FSRSensor* LFoot_front_left_; 97 | const Sim::FSRSensor* LFoot_front_right_; 98 | const Sim::FSRSensor* LFoot_rear_left_; 99 | const Sim::FSRSensor* LFoot_rear_right_; 100 | const Sim::FSRSensor* RFoot_front_left_; 101 | const Sim::FSRSensor* RFoot_front_right_; 102 | const Sim::FSRSensor* RFoot_rear_left_; 103 | const Sim::FSRSensor* RFoot_rear_right_; 104 | 105 | // ROS Parameters 106 | std::vector pid_controllers_; 107 | 108 | // Gazebo joints/sensors 109 | std::vector gazebo_joints_; 110 | sensors::CameraSensorPtr gazebo_top_camera_, gazebo_bottom_camera_; 111 | sensors::ImuSensorPtr gazebo_imu_; 112 | sensors::ContactSensorPtr gazebo_lfoot_front_left_, gazebo_lfoot_front_right_, gazebo_lfoot_rear_left_, gazebo_lfoot_rear_right_; 113 | sensors::ContactSensorPtr gazebo_rfoot_front_left_, gazebo_rfoot_front_right_, gazebo_rfoot_rear_left_, gazebo_rfoot_rear_right_; 114 | sensors::RaySensorPtr gazebo_left_sonar_, gazebo_right_sonar_; 115 | 116 | // Pointer to the model 117 | physics::ModelPtr model_; 118 | 119 | // Pointer to the world 120 | physics::WorldPtr world_; 121 | 122 | // Pointer to the update event connection 123 | event::ConnectionPtr update_connection_; 124 | 125 | event::ConnectionPtr new_top_camera_frame_connection_, new_bottom_camera_frame_connection_; 126 | event::ConnectionPtr imu_update_connection_; 127 | event::ConnectionPtr left_sonar_update_connection_, right_sonar_update_connection_; 128 | event::ConnectionPtr fsr_lfl_update_connection_, fsr_lfr_update_connection_, fsr_lrl_update_connection_, fsr_lrr_update_connection_; 129 | event::ConnectionPtr fsr_rfl_update_connection_, fsr_rfr_update_connection_, fsr_rrl_update_connection_, fsr_rrr_update_connection_; 130 | 131 | // Gazebo/ROS Parameters 132 | ros::Duration control_period_; 133 | ros::Time last_update_sim_time_ros_, last_write_sim_time_ros_; 134 | math::Vector3 gravity_; 135 | 136 | std::vector joints_names_; 137 | 138 | std::string naoqi_path_, naoqi_sim_path_, naoqi_model_type_; 139 | std::string robot_namespace_; 140 | int naoqi_port_; 141 | 142 | }; 143 | } 144 | 145 | #endif 146 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/config/urdf.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 | - /RobotModel1 10 | - /TF1 11 | Splitter Ratio: 0.5 12 | Tree Height: 565 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 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.03 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: 0.7 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | CameraTop_frame: 60 | Alpha: 1 61 | Show Axes: false 62 | Show Trail: false 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Head: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | LBicep: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | LElbow: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | LFeet: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | LFinger11: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | LFinger12: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | LFinger13: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | LFinger21: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | LFinger22: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | LFinger23: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | LForeArm: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | Value: true 120 | LHip: 121 | Alpha: 1 122 | Show Axes: false 123 | Show Trail: false 124 | Value: true 125 | LPelvis: 126 | Alpha: 1 127 | Show Axes: false 128 | Show Trail: false 129 | Value: true 130 | LShoulder: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | LSonar: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | LThigh: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | Value: true 144 | LThumb1: 145 | Alpha: 1 146 | Show Axes: false 147 | Show Trail: false 148 | Value: true 149 | LThumb2: 150 | Alpha: 1 151 | Show Axes: false 152 | Show Trail: false 153 | Value: true 154 | LTibia: 155 | Alpha: 1 156 | Show Axes: false 157 | Show Trail: false 158 | Value: true 159 | Link Tree Style: Links in Alphabetic Order 160 | Neck: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | RBicep: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | Value: true 170 | RElbow: 171 | Alpha: 1 172 | Show Axes: false 173 | Show Trail: false 174 | RFeet: 175 | Alpha: 1 176 | Show Axes: false 177 | Show Trail: false 178 | Value: true 179 | RFinger11: 180 | Alpha: 1 181 | Show Axes: false 182 | Show Trail: false 183 | Value: true 184 | RFinger12: 185 | Alpha: 1 186 | Show Axes: false 187 | Show Trail: false 188 | Value: true 189 | RFinger13: 190 | Alpha: 1 191 | Show Axes: false 192 | Show Trail: false 193 | Value: true 194 | RFinger21: 195 | Alpha: 1 196 | Show Axes: false 197 | Show Trail: false 198 | Value: true 199 | RFinger22: 200 | Alpha: 1 201 | Show Axes: false 202 | Show Trail: false 203 | Value: true 204 | RFinger23: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | Value: true 209 | RForeArm: 210 | Alpha: 1 211 | Show Axes: false 212 | Show Trail: false 213 | Value: true 214 | RHip: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | Value: true 219 | RPelvis: 220 | Alpha: 1 221 | Show Axes: false 222 | Show Trail: false 223 | Value: true 224 | RShoulder: 225 | Alpha: 1 226 | Show Axes: false 227 | Show Trail: false 228 | Value: true 229 | RSonar: 230 | Alpha: 1 231 | Show Axes: false 232 | Show Trail: false 233 | RThigh: 234 | Alpha: 1 235 | Show Axes: false 236 | Show Trail: false 237 | Value: true 238 | RThumb1: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | Value: true 243 | RThumb2: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | Value: true 248 | RTibia: 249 | Alpha: 1 250 | Show Axes: false 251 | Show Trail: false 252 | Value: true 253 | base_link: 254 | Alpha: 1 255 | Show Axes: false 256 | Show Trail: false 257 | gaze: 258 | Alpha: 1 259 | Show Axes: false 260 | Show Trail: false 261 | l_ankle: 262 | Alpha: 1 263 | Show Axes: false 264 | Show Trail: false 265 | Value: true 266 | l_gripper: 267 | Alpha: 1 268 | Show Axes: false 269 | Show Trail: false 270 | l_sole: 271 | Alpha: 1 272 | Show Axes: false 273 | Show Trail: false 274 | l_wrist: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | r_ankle: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | Value: true 284 | r_gripper: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | r_sole: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | r_wrist: 293 | Alpha: 1 294 | Show Axes: false 295 | Show Trail: false 296 | Value: true 297 | torso: 298 | Alpha: 1 299 | Show Axes: false 300 | Show Trail: false 301 | Value: true 302 | Name: RobotModel 303 | Robot Description: robot_description 304 | TF Prefix: "" 305 | Update Interval: 0 306 | Value: true 307 | Visual Enabled: true 308 | - Class: rviz/TF 309 | Enabled: true 310 | Frame Timeout: 15 311 | Frames: 312 | All Enabled: true 313 | CameraTop_frame: 314 | Value: true 315 | Head: 316 | Value: true 317 | LBicep: 318 | Value: true 319 | LElbow: 320 | Value: true 321 | LFeet: 322 | Value: true 323 | LFinger11: 324 | Value: true 325 | LFinger12: 326 | Value: true 327 | LFinger13: 328 | Value: true 329 | LFinger21: 330 | Value: true 331 | LFinger22: 332 | Value: true 333 | LFinger23: 334 | Value: true 335 | LForeArm: 336 | Value: true 337 | LHip: 338 | Value: true 339 | LPelvis: 340 | Value: true 341 | LShoulder: 342 | Value: true 343 | LSonar: 344 | Value: true 345 | LThigh: 346 | Value: true 347 | LThumb1: 348 | Value: true 349 | LThumb2: 350 | Value: true 351 | LTibia: 352 | Value: true 353 | Neck: 354 | Value: true 355 | RBicep: 356 | Value: true 357 | RElbow: 358 | Value: true 359 | RFeet: 360 | Value: true 361 | RFinger11: 362 | Value: true 363 | RFinger12: 364 | Value: true 365 | RFinger13: 366 | Value: true 367 | RFinger21: 368 | Value: true 369 | RFinger22: 370 | Value: true 371 | RFinger23: 372 | Value: true 373 | RForeArm: 374 | Value: true 375 | RHip: 376 | Value: true 377 | RPelvis: 378 | Value: true 379 | RShoulder: 380 | Value: true 381 | RSonar: 382 | Value: true 383 | RThigh: 384 | Value: true 385 | RThumb1: 386 | Value: true 387 | RThumb2: 388 | Value: true 389 | RTibia: 390 | Value: true 391 | base_link: 392 | Value: true 393 | gaze: 394 | Value: true 395 | l_ankle: 396 | Value: true 397 | l_gripper: 398 | Value: true 399 | l_sole: 400 | Value: true 401 | l_wrist: 402 | Value: true 403 | r_ankle: 404 | Value: true 405 | r_gripper: 406 | Value: true 407 | r_sole: 408 | Value: true 409 | r_wrist: 410 | Value: true 411 | torso: 412 | Value: true 413 | Marker Scale: 0.3 414 | Name: TF 415 | Show Arrows: true 416 | Show Axes: true 417 | Show Names: true 418 | Tree: 419 | base_link: 420 | torso: 421 | LPelvis: 422 | LHip: 423 | LThigh: 424 | LTibia: 425 | l_ankle: 426 | LFeet: 427 | l_sole: 428 | {} 429 | LShoulder: 430 | LBicep: 431 | LElbow: 432 | LForeArm: 433 | l_wrist: 434 | LFinger11: 435 | LFinger12: 436 | LFinger13: 437 | {} 438 | LFinger21: 439 | LFinger22: 440 | LFinger23: 441 | {} 442 | LThumb1: 443 | LThumb2: 444 | {} 445 | l_gripper: 446 | {} 447 | LSonar: 448 | {} 449 | Neck: 450 | Head: 451 | CameraTop_frame: 452 | gaze: 453 | {} 454 | RPelvis: 455 | RHip: 456 | RThigh: 457 | RTibia: 458 | r_ankle: 459 | RFeet: 460 | r_sole: 461 | {} 462 | RShoulder: 463 | RBicep: 464 | RElbow: 465 | RForeArm: 466 | r_wrist: 467 | RFinger11: 468 | RFinger12: 469 | RFinger13: 470 | {} 471 | RFinger21: 472 | RFinger22: 473 | RFinger23: 474 | {} 475 | RThumb1: 476 | RThumb2: 477 | {} 478 | r_gripper: 479 | {} 480 | RSonar: 481 | {} 482 | Update Interval: 0 483 | Value: true 484 | Enabled: true 485 | Global Options: 486 | Background Color: 48; 48; 48 487 | Fixed Frame: torso 488 | Frame Rate: 30 489 | Name: root 490 | Tools: 491 | - Class: rviz/Interact 492 | Hide Inactive Objects: true 493 | - Class: rviz/MoveCamera 494 | - Class: rviz/Select 495 | - Class: rviz/FocusCamera 496 | - Class: rviz/Measure 497 | - Class: rviz/SetInitialPose 498 | Topic: /initialpose 499 | - Class: rviz/SetGoal 500 | Topic: /move_base_simple/goal 501 | - Class: rviz/PublishPoint 502 | Single click: true 503 | Topic: /clicked_point 504 | Value: true 505 | Views: 506 | Current: 507 | Class: rviz/Orbit 508 | Distance: 1.32784 509 | Enable Stereo Rendering: 510 | Stereo Eye Separation: 0.06 511 | Stereo Focal Distance: 1 512 | Swap Stereo Eyes: false 513 | Value: false 514 | Focal Point: 515 | X: 0 516 | Y: 0 517 | Z: 0 518 | Name: Current View 519 | Near Clip Distance: 0.01 520 | Pitch: 0.055398 521 | Target Frame: 522 | Value: Orbit (rviz) 523 | Yaw: 0.000398632 524 | Saved: ~ 525 | Window Geometry: 526 | Displays: 527 | collapsed: false 528 | Height: 846 529 | Hide Left Dock: false 530 | Hide Right Dock: false 531 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 532 | Selection: 533 | collapsed: false 534 | Time: 535 | collapsed: false 536 | Tool Properties: 537 | collapsed: false 538 | Views: 539 | collapsed: false 540 | Width: 1200 541 | X: 55 542 | Y: 40 543 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/src/gazebo_naoqi_control_plugin.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | Copyright (c) 2015, Konstantinos Chatzilygeroudis 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 9 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 10 | in the documentation and/or other materials provided with the distribution. 11 | 12 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 13 | from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 16 | BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 17 | SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 18 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 19 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 20 | OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 21 | **/ 22 | 23 | // Includes 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace gazebo 31 | { 32 | 33 | GazeboNaoqiControlPlugin::GazeboNaoqiControlPlugin() 34 | { 35 | // Create NAOqi simulation launcher 36 | naoqi_sim_launcher_ = new Sim::SimLauncher(); 37 | 38 | // Get paths from CMake 39 | naoqi_path_ = NAOQI_SDK; 40 | naoqi_sim_path_ = NAOQI_SIM_SDK; 41 | } 42 | 43 | GazeboNaoqiControlPlugin::~GazeboNaoqiControlPlugin() 44 | { 45 | // Disconnect from Gazebo Update Loop 46 | event::Events::DisconnectWorldUpdateBegin(update_connection_); 47 | 48 | // Disconnect from Sensors Update Loops 49 | if(gazebo_top_camera_) 50 | gazebo_top_camera_->GetCamera()->DisconnectNewImageFrame(new_top_camera_frame_connection_); 51 | if(gazebo_bottom_camera_) 52 | gazebo_bottom_camera_->GetCamera()->DisconnectNewImageFrame(new_bottom_camera_frame_connection_); 53 | 54 | if(gazebo_imu_) 55 | gazebo_imu_->DisconnectUpdated(imu_update_connection_); 56 | 57 | if(gazebo_left_sonar_) 58 | gazebo_left_sonar_->GetLaserShape()->DisconnectNewLaserScans(left_sonar_update_connection_); 59 | if(gazebo_right_sonar_) 60 | gazebo_right_sonar_->GetLaserShape()->DisconnectNewLaserScans(right_sonar_update_connection_); 61 | 62 | if(gazebo_lfoot_front_left_) 63 | gazebo_lfoot_front_left_->DisconnectUpdated(fsr_lfl_update_connection_); 64 | if(gazebo_lfoot_front_right_) 65 | gazebo_lfoot_front_right_->DisconnectUpdated(fsr_lfr_update_connection_); 66 | if(gazebo_lfoot_rear_left_) 67 | gazebo_lfoot_rear_left_->DisconnectUpdated(fsr_lrl_update_connection_); 68 | if(gazebo_lfoot_rear_right_) 69 | gazebo_lfoot_rear_right_->DisconnectUpdated(fsr_lrr_update_connection_); 70 | 71 | if(gazebo_rfoot_front_left_) 72 | gazebo_rfoot_front_left_->DisconnectUpdated(fsr_rfl_update_connection_); 73 | if(gazebo_rfoot_front_right_) 74 | gazebo_rfoot_front_right_->DisconnectUpdated(fsr_rfr_update_connection_); 75 | if(gazebo_rfoot_rear_left_) 76 | gazebo_rfoot_rear_left_->DisconnectUpdated(fsr_rrl_update_connection_); 77 | if(gazebo_rfoot_rear_right_) 78 | gazebo_rfoot_rear_right_->DisconnectUpdated(fsr_rrr_update_connection_); 79 | 80 | // Delete Simulation Objects if necessary 81 | if(naoqi_sim_launcher_) 82 | delete naoqi_sim_launcher_; 83 | if(naoqi_model_) 84 | delete naoqi_model_; 85 | if(naoqi_hal_) 86 | delete naoqi_hal_; 87 | } 88 | 89 | void GazeboNaoqiControlPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf ) 90 | { 91 | ros::NodeHandle model_nh; 92 | model_ = _parent; 93 | 94 | // Error message if the model couldn't be found 95 | if (!model_) 96 | { 97 | ROS_ERROR("Parent model is NULL! GazeboNaoqiControlPlugin could not be loaded."); 98 | return; 99 | } 100 | 101 | // Check that ROS has been initialized 102 | if(!ros::isInitialized()) 103 | { 104 | ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin."); 105 | return; 106 | } 107 | 108 | // Get Gazebo World pointer 109 | world_ = model_->GetWorld(); 110 | 111 | // Get the Gazebo simulation period 112 | ros::Duration gazebo_period(world_->GetPhysicsEngine()->GetMaxStepSize()); 113 | 114 | // Get Gravity vector 115 | gravity_ = world_->GetPhysicsEngine()->GetGravity(); 116 | 117 | // Check for robot namespace 118 | robot_namespace_ = "/"; 119 | if(_sdf->HasElement("robotNamespace")) 120 | { 121 | robot_namespace_ = _sdf->GetElement("robotNamespace")->Get(); 122 | } 123 | 124 | // Check for controlPeriod element 125 | if(_sdf->HasElement("controlPeriod")) 126 | { 127 | control_period_ = ros::Duration(_sdf->Get("controlPeriod")); 128 | 129 | // Check the period against the simulation period 130 | if(control_period_ < gazebo_period) 131 | { 132 | ROS_ERROR("Desired controller update period is faster than the gazebo simulation period."); 133 | control_period_ = gazebo_period; 134 | } 135 | else if(control_period_ > gazebo_period) 136 | { 137 | ROS_WARN("Desired controller update period is slower than the gazebo simulation period."); 138 | } 139 | } 140 | else 141 | { 142 | control_period_ = gazebo_period; 143 | } 144 | 145 | // Check for port element 146 | if(_sdf->HasElement("port")) 147 | { 148 | naoqi_port_ = _sdf->GetElement("port")->Get(); 149 | } 150 | else 151 | { 152 | naoqi_port_ = 9559; 153 | ROS_WARN("No port element present. Using default: [%d]", naoqi_port_); 154 | } 155 | 156 | // Check for modelType element 157 | naoqi_model_type_ = "NAOH25V40"; 158 | if(_sdf->HasElement("modelType")) 159 | { 160 | naoqi_model_type_ = _sdf->GetElement("modelType")->Get(); 161 | } 162 | 163 | // Load NAOqi model and start simulated NAOqi 164 | try 165 | { 166 | boost::filesystem::path naoqi_sim_path = naoqi_sim_path_; 167 | naoqi_sim_path /= "share/alrobotmodel/models/"+naoqi_model_type_+".xml"; 168 | // Get Model from Simulation SDK dir 169 | naoqi_model_ = new Sim::Model(naoqi_sim_path.string()); 170 | // Create HAL interface 171 | naoqi_hal_ = new Sim::HALInterface(naoqi_model_, naoqi_port_); 172 | // Launch Simulation 173 | if(!naoqi_sim_launcher_->launch(naoqi_model_, naoqi_port_, naoqi_sim_path_)) 174 | { 175 | ROS_ERROR("Failed to Launch HAL or NAOqi. GazeboNaoqiControlPlugin could not be loaded."); 176 | return; 177 | } 178 | } 179 | catch(const AL::ALError& e) 180 | { 181 | ROS_ERROR("Exception while launching HAL or NAOqi. GazeboNaoqiControlPlugin could not be loaded.\n\t%s", e.what()); 182 | return; 183 | } 184 | 185 | // Get actuators/sensors from NAOqi 186 | angle_sensors_ = naoqi_model_->angleSensors(); 187 | 188 | angle_actuators_ = naoqi_model_->angleActuators(); 189 | 190 | joints_ = naoqi_model_->joints(); 191 | // Create Gazebo Joints 192 | for(unsigned int i=0;iname(); 195 | physics::JointPtr joint = model_->GetJoint(name); 196 | if(joint) 197 | { 198 | gazebo_joints_.push_back(joint); 199 | joints_names_.push_back(name); 200 | const ros::NodeHandle nh(model_nh, std::string(robot_namespace_+"/gazebo_ros_control/pid_gains/")+name); 201 | double p, i,d ; 202 | // TO-DO: include i_clamp e.t.c. 203 | nh.param("p", p, 0.0); 204 | nh.param("i", i, 0.0); 205 | nh.param("d", d, 0.0); 206 | 207 | pid_controllers_.push_back(control_toolbox::Pid(p, i, d)); 208 | } 209 | } 210 | 211 | // Initialize actuators 212 | for(std::vector::const_iterator it = 213 | angle_actuators_.begin(); it != angle_actuators_.end(); ++it) 214 | { 215 | float actuatorPosition = naoqi_hal_->fetchAngleActuatorValue(*it); 216 | if(actuatorPosition != actuatorPosition) 217 | { 218 | actuatorPosition = (*it)->startValue(); 219 | } 220 | const Sim::AngleSensor* sensor = naoqi_model_->angleSensor((*it)->name()); 221 | naoqi_hal_->sendAngleSensorValue(sensor, actuatorPosition); 222 | } 223 | 224 | // Initialize Sensors 225 | initSensors(); 226 | 227 | 228 | // Listen to the update event. This event is broadcast every 229 | // simulation iteration. 230 | update_connection_ = event::Events::ConnectWorldUpdateBegin( 231 | boost::bind(&GazeboNaoqiControlPlugin::Update, this)); 232 | 233 | ROS_INFO("GazeboNaoqiControlPlugin loaded successfully!"); 234 | } 235 | 236 | void GazeboNaoqiControlPlugin::initSensors() 237 | { 238 | // Activate Camera Sensors 239 | std::vector camera_sensors = naoqi_model_->cameraSensors(); 240 | for(int i=0;i(sensors::SensorManager::Instance()->GetSensor(camera_sensors[i]->name()))); 243 | 244 | if(cam) 245 | { 246 | // Subscribe to camera updates 247 | if(camera_sensors[i]->name() == "CameraTop") 248 | { 249 | top_camera_ = camera_sensors[i]; 250 | gazebo_top_camera_ = cam; 251 | new_top_camera_frame_connection_ = cam->GetCamera()->ConnectNewImageFrame(boost::bind(&GazeboNaoqiControlPlugin::onCameraUpdate, this, top_camera_, _1, _2, _3, _4, _5)); 252 | } 253 | else if(camera_sensors[i]->name() == "CameraBottom") 254 | { 255 | bottom_camera_ = camera_sensors[i]; 256 | gazebo_bottom_camera_ = cam; 257 | new_bottom_camera_frame_connection_ = cam->GetCamera()->ConnectNewImageFrame(boost::bind(&GazeboNaoqiControlPlugin::onCameraUpdate, this, bottom_camera_, _1, _2, _3, _4, _5)); 258 | } 259 | else 260 | continue; 261 | cam->SetActive(true); 262 | } 263 | } 264 | 265 | // Activate IMU - I assume only 1 IMU (TO-DO: Check cases where more exist) 266 | std::vector inertial_sensors = naoqi_model_->inertialSensors(); 267 | if(inertial_sensors.size()>=1) 268 | { 269 | sensors::ImuSensorPtr imu = (boost::dynamic_pointer_cast(sensors::SensorManager::Instance()->GetSensor("imu"))); 270 | 271 | if(imu) 272 | { 273 | imu_update_connection_ = imu->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onImuUpdate, this, imu)); 274 | gazebo_imu_ = imu; 275 | inertial_sensor_ = inertial_sensors[0]; 276 | imu->SetActive(true); 277 | } 278 | } 279 | 280 | // Activate FSRs 281 | std::vector fsr_sensors = naoqi_model_->fsrSensors(); 282 | for(int i=0;i(sensors::SensorManager::Instance()->GetSensor(fsr_sensors[i]->name()))); 285 | 286 | if(fsr) 287 | { 288 | std::string name = fsr_sensors[i]->name(); 289 | if(name == "RFoot/FSR/RearLeft") 290 | { 291 | gazebo_rfoot_rear_left_ = fsr; 292 | RFoot_rear_left_ = fsr_sensors[i]; 293 | fsr_rrl_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_rfoot_rear_left_, RFoot_rear_left_)); 294 | } 295 | else if(name == "RFoot/FSR/RearRight") 296 | { 297 | gazebo_rfoot_rear_right_ = fsr; 298 | RFoot_rear_right_ = fsr_sensors[i]; 299 | fsr_rrr_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_rfoot_rear_right_, RFoot_rear_right_)); 300 | } 301 | else if(name == "RFoot/FSR/FrontLeft") 302 | { 303 | gazebo_rfoot_front_left_ = fsr; 304 | RFoot_front_left_ = fsr_sensors[i]; 305 | fsr_rfl_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_rfoot_front_left_, RFoot_front_left_)); 306 | } 307 | else if(name == "RFoot/FSR/FrontRight") 308 | { 309 | gazebo_rfoot_front_right_ = fsr; 310 | RFoot_front_right_ = fsr_sensors[i]; 311 | fsr_rfr_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_rfoot_front_right_, RFoot_front_right_)); 312 | } 313 | else if(name == "LFoot/FSR/RearLeft") 314 | { 315 | gazebo_lfoot_rear_left_ = fsr; 316 | LFoot_rear_left_ = fsr_sensors[i]; 317 | fsr_lrl_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_lfoot_rear_left_, LFoot_rear_left_)); 318 | } 319 | else if(name == "LFoot/FSR/RearRight") 320 | { 321 | gazebo_lfoot_rear_right_ = fsr; 322 | LFoot_rear_right_ = fsr_sensors[i]; 323 | fsr_lrr_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_lfoot_rear_right_, LFoot_rear_right_)); 324 | } 325 | else if(name == "LFoot/FSR/FrontLeft") 326 | { 327 | gazebo_lfoot_front_left_ = fsr; 328 | LFoot_front_left_ = fsr_sensors[i]; 329 | fsr_lfl_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_lfoot_front_left_, LFoot_front_left_)); 330 | } 331 | else if(name == "LFoot/FSR/FrontRight") 332 | { 333 | gazebo_lfoot_front_right_ = fsr; 334 | LFoot_front_right_ = fsr_sensors[i]; 335 | fsr_lfr_update_connection_ = fsr->ConnectUpdated(boost::bind(&GazeboNaoqiControlPlugin::onFSRUpdate, this, gazebo_lfoot_front_right_, LFoot_front_right_)); 336 | } 337 | fsr->SetActive(true); 338 | } 339 | } 340 | 341 | // Activate Sonars 342 | std::vector sonar_sensors = naoqi_model_->sonarSensors(); 343 | for(int i=0;i(sensors::SensorManager::Instance()->GetSensor(sonar_sensors[i]->name()))); 346 | if(sonar) 347 | { 348 | if(sonar_sensors[i]->name() == "Sonar/Left") 349 | { 350 | left_sonar_ = sonar_sensors[i]; 351 | gazebo_left_sonar_ = sonar; 352 | left_sonar_update_connection_ = sonar->GetLaserShape()->ConnectNewLaserScans(boost::bind(&GazeboNaoqiControlPlugin::onSonarUpdate, this, gazebo_left_sonar_, left_sonar_)); 353 | } 354 | else if(sonar_sensors[i]->name() == "Sonar/Right") 355 | { 356 | right_sonar_ = sonar_sensors[i]; 357 | gazebo_right_sonar_ = sonar; 358 | right_sonar_update_connection_ = sonar->GetLaserShape()->ConnectNewLaserScans(boost::bind(&GazeboNaoqiControlPlugin::onSonarUpdate, this, gazebo_right_sonar_, right_sonar_)); 359 | } 360 | sonar->SetActive(true); 361 | } 362 | } 363 | } 364 | 365 | void GazeboNaoqiControlPlugin::onCameraUpdate(const Sim::CameraSensor* _camera, const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format) 366 | { 367 | int res; 368 | if(_width==80) 369 | res = Sim::RES_80_60; 370 | else if(_width==160) 371 | res = Sim::RES_160_120; 372 | else if(_width==320) 373 | res = Sim::RES_320_240; 374 | else if(_width==640) 375 | res = Sim::RES_640_480; 376 | else if(_width==1280) 377 | res = Sim::RES_1280_960; 378 | else 379 | res = Sim::RES_UNKNOWN; 380 | 381 | // TO-DO: Update Gazebo camera to match NAOqi's resolution 382 | int r = naoqi_hal_->cameraResolution(_camera); 383 | if(r!=res) 384 | { 385 | ROS_WARN("Mismatch in dimensions when sending image to camera '%s': %d vs %d", _camera->name().c_str(), res, r); 386 | return; 387 | } 388 | // Send image to NAOqi 389 | naoqi_hal_->sendCameraSensorValue(_camera, _image, (Sim::CameraResolution)res, Sim::COL_SPACE_RGB); 390 | } 391 | 392 | void GazeboNaoqiControlPlugin::onImuUpdate(sensors::ImuSensorPtr _sensor) 393 | { 394 | //Update IMU 395 | // Get IMU data from Gazebo 396 | math::Vector3 gyro = gazebo_imu_->GetAngularVelocity(); 397 | math::Vector3 acc = gazebo_imu_->GetLinearAcceleration(); 398 | math::Quaternion orient = gazebo_imu_->GetOrientation(); 399 | 400 | std::vector vals; 401 | // AngleX, AngleY, [AngleZ - not in V40], AccX, AccY, AccZ, GyroX, GyroY, [GyroZ - not in V40] 402 | 403 | vals.push_back(orient.GetRoll()); 404 | vals.push_back(orient.GetPitch()); 405 | // if(naoqi_model_type_ == "NAOH25V50") //Should be working for V50 - it doesn't 406 | // vals.push_back(orient.GetYaw()); 407 | vals.push_back(acc[0]); 408 | vals.push_back(acc[1]); 409 | vals.push_back(-acc[2]); 410 | vals.push_back(gyro[0]); 411 | vals.push_back(gyro[1]); 412 | // if(naoqi_model_type_ == "NAOH25V50") //Should be working for V50 - it doesn't 413 | // vals.push_back(gyro[2]); 414 | 415 | // Send IMU data to NAOqi 416 | naoqi_hal_->sendInertialSensorValues(inertial_sensor_, vals); 417 | } 418 | 419 | void GazeboNaoqiControlPlugin::onSonarUpdate(sensors::RaySensorPtr _gazebo_sonar, const Sim::SonarSensor* _sonar) 420 | { 421 | // Update Left Sonar 422 | // Get Sonar range from Gazebo 423 | double val = 0.0; 424 | int N = _gazebo_sonar->GetRayCount(); 425 | for(int i=0;iGetRange(i); 428 | } 429 | val /= double(N); 430 | 431 | // Send Sonar range to NAOqi 432 | naoqi_hal_->sendSonarSensorValue(_sonar, float(val)); 433 | } 434 | 435 | void GazeboNaoqiControlPlugin::onFSRUpdate(sensors::ContactSensorPtr _gazebo_fsr, const Sim::FSRSensor* _fsr) 436 | { 437 | // Update FSR 438 | // Get FSR data from Gazebo 439 | msgs::Contacts contacts = _gazebo_fsr->GetContacts(); 440 | math::Vector3 force = math::Vector3::Zero; 441 | for(int j=0;jsendFSRSensorValue(_fsr, force[2]/g); 460 | } 461 | 462 | void GazeboNaoqiControlPlugin::Update() 463 | { 464 | // Get the simulation time and period 465 | gazebo::common::Time gz_time_now = world_->GetSimTime(); 466 | ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec); 467 | ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; 468 | 469 | // Check if we should update the controllers 470 | if(sim_period >= control_period_) { 471 | // Store this simulation time 472 | last_update_sim_time_ros_ = sim_time_ros; 473 | 474 | // Update the robot simulation with the state of the gazebo model 475 | readSim(); 476 | } 477 | 478 | // Update the gazebo model with commands from NAOqi 479 | writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_); 480 | last_write_sim_time_ros_ = sim_time_ros; 481 | } 482 | 483 | void GazeboNaoqiControlPlugin::readSim() 484 | { 485 | // Read joint states from Gazebo and send them to NAOqi 486 | for(unsigned int i=0;iGetAngle(0).Radian(); 489 | if(angle!=angle) 490 | continue; 491 | if(naoqi_model_->angleSensor(joints_names_[i])) //sometimes we cannot get the handle 492 | naoqi_hal_->sendAngleSensorValue(naoqi_model_->angleSensor(joints_names_[i]), angle); 493 | } 494 | } 495 | 496 | void GazeboNaoqiControlPlugin::writeSim(ros::Time time, ros::Duration period) 497 | { 498 | // Get actuator commands from NAOqi and write them in Gazebo 499 | // TO-DO: Implement LED, Sonar and other commads 500 | for(unsigned int i=0;iangleActuator(joints_names_[i])) //sometimes we cannot get the handle 503 | continue; 504 | 505 | double angle = naoqi_hal_->fetchAngleActuatorValue(naoqi_model_->angleActuator(joints_names_[i])); 506 | if(angle!=angle) 507 | { 508 | angle = naoqi_model_->angleActuator(joints_names_[i])->startValue(); 509 | } 510 | double a = gazebo_joints_[i]->GetAngle(0).Radian(); 511 | if(a!=a) 512 | a = angle; 513 | double error = angle-a; 514 | double effort = gazebo::math::clamp(pid_controllers_[i].computeCommand(error, period), -2.0, 2.0); 515 | gazebo_joints_[i]->SetForce(0, effort); 516 | } 517 | } 518 | 519 | GZ_REGISTER_MODEL_PLUGIN(GazeboNaoqiControlPlugin); 520 | 521 | } 522 | -------------------------------------------------------------------------------- /gazebo_naoqi_control/models/nao.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | ${robotNamespace} 22 | 23 | ${controlPeriod} 24 | 25 | ${port} 26 | ${modelType} 27 | 28 | 29 | 30 | 31 | 32 | 0.5 33 | 0.5 34 | false 35 | false 36 | 37 | 38 | 0.5 39 | 0.5 40 | false 41 | false 42 | 43 | 44 | 0.5 45 | 0.5 46 | false 47 | false 48 | 49 | 50 | 0.5 51 | 0.5 52 | false 53 | false 54 | 55 | 56 | 0.5 57 | 0.5 58 | false 59 | false 60 | 61 | 62 | 0.5 63 | 0.5 64 | false 65 | false 66 | 67 | 68 | 0.5 69 | 0.5 70 | false 71 | false 72 | 73 | 74 | 0.5 75 | 0.5 76 | false 77 | false 78 | 79 | 80 | 0.5 81 | 0.5 82 | false 83 | false 84 | 85 | 86 | 0.5 87 | 0.5 88 | false 89 | true 90 | 91 | 92 | 0.5 93 | 0.5 94 | false 95 | false 96 | 97 | 98 | 0.5 99 | 0.5 100 | false 101 | false 102 | 103 | 104 | 0.5 105 | 0.5 106 | false 107 | false 108 | 109 | 110 | 0.5 111 | 0.5 112 | false 113 | false 114 | 115 | 116 | 0.5 117 | 0.5 118 | false 119 | false 120 | 121 | 122 | 0.5 123 | 0.5 124 | false 125 | true 126 | 127 | 128 | 0.5 129 | 0.5 130 | false 131 | true 132 | 133 | 134 | 0.5 135 | 0.5 136 | false 137 | true 138 | 139 | 140 | 0.5 141 | 0.5 142 | false 143 | true 144 | 145 | 146 | 0.5 147 | 0.5 148 | false 149 | true 150 | 151 | 152 | 0.5 153 | 0.5 154 | false 155 | true 156 | 157 | 158 | 0.5 159 | 0.5 160 | false 161 | true 162 | 163 | 164 | 0.5 165 | 0.5 166 | false 167 | true 168 | 169 | 170 | 0.5 171 | 0.5 172 | false 173 | true 174 | 175 | 176 | 0.5 177 | 0.5 178 | false 179 | false 180 | 181 | 182 | 0.5 183 | 0.5 184 | false 185 | false 186 | 187 | 188 | 0.5 189 | 0.5 190 | false 191 | false 192 | 193 | 194 | 0.5 195 | 0.5 196 | false 197 | false 198 | 199 | 200 | 0.5 201 | 0.5 202 | false 203 | false 204 | 205 | 206 | 0.5 207 | 0.5 208 | false 209 | false 210 | 1 0 0 211 | 0.1 212 | 0.003 213 | 214 | 215 | 0.5 216 | 0.5 217 | false 218 | false 219 | 1 0 0 220 | 0.1 221 | 0.003 222 | 223 | 224 | 0.5 225 | 0.5 226 | false 227 | false 228 | 229 | 230 | 0.5 231 | 0.5 232 | false 233 | false 234 | 235 | 236 | 0.5 237 | 0.5 238 | false 239 | false 240 | 241 | 242 | 0.5 243 | 0.5 244 | false 245 | false 246 | 247 | 248 | 0.5 249 | 0.5 250 | false 251 | false 252 | 253 | 254 | 0.5 255 | 0.5 256 | false 257 | false 258 | 1 0 0 259 | 0.1 260 | 0.003 261 | 262 | 263 | 0.5 264 | 0.5 265 | false 266 | false 267 | 1 0 0 268 | 0.1 269 | 0.003 270 | 271 | 272 | 0.5 273 | 0.5 274 | false 275 | true 276 | 277 | 278 | 0.5 279 | 0.5 280 | false 281 | true 282 | 283 | 284 | 0.5 285 | 0.5 286 | false 287 | true 288 | 289 | 290 | 0.5 291 | 0.5 292 | false 293 | true 294 | 295 | 296 | 0.5 297 | 0.5 298 | false 299 | true 300 | 301 | 302 | 0.5 303 | 0.5 304 | false 305 | true 306 | 307 | 308 | 0.5 309 | 0.5 310 | false 311 | true 312 | 313 | 314 | 0.5 315 | 0.5 316 | false 317 | true 318 | 319 | 320 | 321 | 30.0 322 | 1 323 | CameraTop 324 | 325 | 1.06290551 326 | 327 | 640 328 | 480 329 | R8G8B8 330 | 331 | 332 | 0.05 333 | 500 334 | 335 | 336 | gaussian 337 | 0.0 338 | 0.007 339 | 340 | 341 | 342 | 343 | 344 | 345 | 30.0 346 | 1 347 | CameraBottom 348 | 349 | 1.06290551 350 | 351 | 320 352 | 240 353 | R8G8B8 354 | 355 | 356 | 0.05 357 | 500 358 | 359 | 360 | gaussian 361 | 0.0 362 | 0.007 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | gaussian 372 | 373 | 0.0 374 | 2e-4 375 | 0.0000075 376 | 0.0000008 377 | 378 | 379 | 0.0 380 | 1.7e-2 381 | 0.1 382 | 0.001 383 | 384 | 385 | 386 | 1 387 | 30 388 | 389 | 390 | 391 | 392 | 0 0 0 0 0 0 393 | 1 394 | 20 395 | false 396 | 397 | 398 | 399 | 5 400 | 1 401 | -0.2617993877991494 402 | 0.2617993877991494 403 | 404 | 405 | 5 406 | 1 407 | -0.13 408 | 0.13 409 | 410 | 411 | 412 | 0.025 413 | 2.55 414 | 1 415 | 416 | 417 | gaussian 418 | 0.0 419 | 0.05 420 | 421 | 422 | 423 | 424 | 425 | 426 | 0 0 0 0 0 0 427 | 1 428 | 20 429 | false 430 | 431 | 432 | 433 | 5 434 | 1 435 | -0.2617993877991494 436 | 0.2617993877991494 437 | 438 | 439 | 5 440 | 1 441 | -0.13 442 | 0.13 443 | 444 | 445 | 446 | 0.025 447 | 2.55 448 | 1 449 | 450 | 451 | gaussian 452 | 0.0 453 | 0.05 454 | 455 | 456 | 457 | 458 | 459 | 460 | 0.5 461 | 0.5 462 | false 463 | false 464 | 1 0 0 465 | 0.1 466 | 0.003 467 | 468 | 1 469 | 20 470 | false 471 | 472 | RFsrRL_frame_collision 473 | RFoot/FSR/RearLeft 474 | 475 | 476 | 477 | 478 | 479 | 0.5 480 | 0.5 481 | false 482 | false 483 | 1 0 0 484 | 0.1 485 | 0.003 486 | 487 | 1 488 | 20 489 | false 490 | 491 | RFsrRR_frame_collision 492 | RFoot/FSR/RearRight 493 | 494 | 495 | 496 | 497 | 498 | 0.5 499 | 0.5 500 | false 501 | false 502 | 1 0 0 503 | 0.1 504 | 0.003 505 | 506 | 1 507 | 20 508 | false 509 | 510 | RFsrFL_frame_collision 511 | RFoot/FSR/FrontLeft 512 | 513 | 514 | 515 | 516 | 517 | 0.5 518 | 0.5 519 | false 520 | false 521 | 1 0 0 522 | 0.1 523 | 0.003 524 | 525 | 1 526 | 20 527 | false 528 | 529 | RFsrFR_frame_collision 530 | RFoot/FSR/FrontRight 531 | 532 | 533 | 534 | 535 | 536 | 0.5 537 | 0.5 538 | false 539 | false 540 | 1 0 0 541 | 0.1 542 | 0.003 543 | 544 | 1 545 | 20 546 | false 547 | 548 | LFsrRL_frame_collision 549 | LFoot/FSR/RearLeft 550 | 551 | 552 | 553 | 554 | 555 | 0.5 556 | 0.5 557 | false 558 | false 559 | 1 0 0 560 | 0.1 561 | 0.003 562 | 563 | 1 564 | 20 565 | false 566 | 567 | LFsrRR_frame_collision 568 | LFoot/FSR/RearRight 569 | 570 | 571 | 572 | 573 | 574 | 0.5 575 | 0.5 576 | false 577 | false 578 | 1 0 0 579 | 0.1 580 | 0.003 581 | 582 | 1 583 | 20 584 | false 585 | 586 | LFsrFL_frame_collision 587 | LFoot/FSR/FrontLeft 588 | 589 | 590 | 591 | 592 | 593 | 0.5 594 | 0.5 595 | false 596 | false 597 | 1 0 0 598 | 0.1 599 | 0.003 600 | 601 | 1 602 | 20 603 | false 604 | 605 | LFsrFR_frame_collision 606 | LFoot/FSR/FrontRight 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 1 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 630 | 631 | 632 | 633 | 634 | 635 | 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | 649 | 650 | 651 | 1 652 | 653 | 654 | 655 | 656 | 657 | 658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 670 | 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 686 | 687 | 688 | 689 | 690 | 691 | 1 692 | 693 | 694 | 695 | 696 | 697 | 698 | 699 | 700 | 701 | 702 | 703 | 704 | 705 | 706 | 707 | 708 | 709 | 710 | 711 | 712 | 713 | 714 | 715 | 716 | 717 | 718 | 719 | 720 | 721 | 1 722 | 723 | 724 | 725 | 726 | 727 | 728 | 729 | 730 | 731 | 732 | 733 | 734 | 735 | 736 | 737 | 738 | 739 | 740 | 741 | 742 | 743 | 744 | 745 | 746 | 747 | 748 | 749 | 750 | 751 | 1 752 | 753 | 754 | 755 | 756 | 757 | 758 | 759 | 760 | 761 | 762 | 763 | 764 | 765 | 766 | 767 | 768 | 769 | 770 | 774 | 775 | 776 | 777 | 778 | 779 | 780 | 781 | 782 | 783 | 784 | 785 | 1 786 | 787 | 788 | 789 | 790 | 791 | 792 | 793 | 794 | 795 | 796 | 797 | 798 | 799 | 800 | 801 | 802 | 803 | 804 | 808 | 809 | 810 | 811 | 812 | 813 | 814 | 815 | 816 | 817 | 818 | 819 | 1 820 | 821 | 822 | 823 | 824 | 825 | 826 | 827 | 828 | 829 | 830 | 831 | 832 | 833 | 834 | 835 | 836 | 837 | 838 | 839 | 840 | 841 | 842 | 843 | 844 | 845 | 846 | 847 | 848 | 849 | 1 850 | 851 | 852 | 853 | 854 | 855 | 856 | 857 | 858 | 859 | 860 | 861 | 862 | 863 | 864 | 868 | 869 | 870 | 871 | 872 | 873 | 874 | 875 | 876 | 877 | 878 | 879 | 880 | 881 | 882 | 883 | 884 | 885 | 886 | 887 | 888 | 889 | 890 | 891 | 1 892 | 893 | 894 | 895 | 896 | 897 | 898 | 899 | 900 | 901 | 902 | 903 | 904 | 905 | 906 | 907 | 908 | 909 | 910 | 911 | 912 | 913 | 914 | 915 | 916 | 917 | 918 | 919 | 920 | 921 | 1 922 | 923 | 924 | 925 | 926 | 927 | 928 | 929 | 930 | 931 | 932 | 933 | 934 | 935 | 936 | 937 | 938 | 939 | 940 | 941 | 942 | 943 | 944 | 945 | 946 | 947 | 948 | 949 | 950 | 951 | 1 952 | 953 | 954 | 955 | 956 | 957 | 958 | 959 | 960 | 961 | 962 | 963 | 964 | 965 | 966 | 967 | 968 | 969 | 970 | 974 | 975 | 976 | 977 | 978 | 979 | 980 | 981 | 982 | 983 | 984 | 985 | 1 986 | 987 | 988 | 989 | 990 | 991 | 992 | 993 | 994 | 995 | 996 | 997 | 998 | 999 | 1000 | 1001 | 1002 | 1003 | 1004 | 1008 | 1009 | 1010 | 1011 | 1012 | 1013 | 1014 | 1015 | 1016 | 1017 | 1018 | 1019 | 1 1020 | 1021 | 1022 | 1023 | 1024 | 1025 | 1026 | 1027 | 1028 | 1029 | 1030 | 1031 | 1032 | 1033 | 1034 | 1035 | 1036 | 1037 | 1038 | 1039 | 1040 | 1041 | 1042 | 1043 | 1044 | 1045 | 1046 | 1047 | 1048 | 1049 | 1 1050 | 1051 | 1052 | 1053 | 1054 | 1055 | 1056 | 1057 | 1058 | 1059 | 1060 | 1061 | 1062 | 1063 | 1064 | 1068 | 1069 | 1070 | 1071 | 1072 | 1073 | 1074 | 1075 | 1076 | 1077 | 1078 | 1079 | 1080 | 1081 | 1082 | 1083 | 1084 | 1085 | 1086 | 1087 | 1088 | 1089 | 1090 | 1091 | 1092 | 1093 | 1094 | 1095 | 1096 | 1097 | 1098 | 1099 | 1100 | 1101 | 1105 | 1106 | 1107 | 1108 | 1109 | 1110 | 1111 | 1112 | 1113 | 1114 | 1115 | 1116 | 1117 | 1118 | 1119 | 1120 | 1 1121 | 1122 | 1123 | 1124 | 1125 | 1126 | 1127 | 1128 | 1129 | 1130 | 1131 | 1132 | 1133 | 1134 | 1135 | 1136 | 1137 | 1138 | 1139 | 1140 | 1141 | 1142 | 1143 | 1144 | 1145 | 1146 | 1147 | 1148 | 1149 | 1150 | 1 1151 | 1152 | 1153 | 1154 | 1155 | 1156 | 1157 | 1158 | 1159 | 1160 | 1161 | 1162 | 1163 | 1164 | 1165 | 1166 | 1167 | 1168 | 1169 | 1173 | 1174 | 1175 | 1176 | 1177 | 1178 | 1179 | 1180 | 1181 | 1182 | 1183 | 1184 | 1 1185 | 1186 | 1187 | 1188 | 1189 | 1190 | 1191 | 1192 | 1193 | 1194 | 1195 | 1196 | 1197 | 1198 | 1199 | 1200 | 1201 | 1202 | 1 1203 | 1204 | 1205 | 1206 | 1207 | 1208 | 1209 | 1210 | 1211 | 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | 1218 | 1219 | 1220 | 1221 | 1225 | 1226 | 1227 | 1228 | 1229 | 1230 | 1231 | 1232 | 1233 | 1234 | 1235 | 1236 | 1 1237 | 1238 | 1239 | 1240 | 1241 | 1242 | 1243 | 1244 | 1245 | 1246 | 1247 | 1248 | 1249 | 1250 | 1251 | 1252 | 1253 | 1254 | 1255 | 1259 | 1260 | 1261 | 1262 | 1263 | 1264 | 1265 | 1266 | 1267 | 1268 | 1269 | 1270 | 1 1271 | 1272 | 1273 | 1274 | 1275 | 1276 | 1277 | 1278 | 1279 | 1280 | 1281 | 1282 | 1283 | 1284 | 1285 | 1286 | 1287 | 1 1288 | 1289 | 1290 | 1291 | 1292 | 1293 | 1294 | 1295 | 1296 | 1297 | 1298 | 1299 | 1300 | 1301 | 1302 | 1303 | 1304 | 1305 | 1306 | 1307 | 1308 | 1309 | 1310 | 1311 | 1312 | 1313 | 1314 | 1315 | 1316 | 1317 | 1 1318 | 1319 | 1320 | 1321 | 1322 | 1323 | 1324 | 1325 | 1326 | 1327 | 1328 | 1329 | 1330 | 1331 | 1332 | 1333 | 1334 | 1335 | 1336 | 1340 | 1341 | 1342 | 1343 | 1344 | 1345 | 1346 | 1347 | 1348 | 1349 | 1350 | 1351 | 1 1352 | 1353 | 1354 | 1355 | 1356 | 1357 | 1358 | 1359 | 1360 | 1361 | 1362 | 1363 | 1364 | 1365 | 1366 | 1367 | 1368 | 1369 | 1 1370 | 1371 | 1372 | 1373 | 1374 | 1375 | 1376 | 1377 | 1378 | 1379 | 1380 | 1381 | 1382 | 1383 | 1384 | 1385 | 1386 | 1387 | 1388 | 1392 | 1393 | 1394 | 1395 | 1396 | 1397 | 1398 | 1399 | 1400 | 1401 | 1402 | 1403 | 1 1404 | 1405 | 1406 | 1407 | 1408 | 1409 | 1410 | 1411 | 1412 | 1413 | 1414 | 1415 | 1416 | 1417 | 1418 | 1419 | 1420 | 1421 | 1422 | 1426 | 1427 | 1428 | 1429 | 1430 | 1431 | 1432 | 1433 | 1434 | 1435 | 1436 | 1437 | 1 1438 | 1439 | 1440 | 1441 | 1442 | 1443 | 1444 | 1445 | 1446 | 1447 | 1448 | 1449 | 1450 | 1451 | 1452 | 1453 | 1454 | 1455 | 1456 | 1457 | 1458 | 1459 | 1460 | 1461 | 1462 | 1463 | 1464 | 1465 | 1466 | 1467 | 1468 | 1469 | 1470 | 1471 | 1472 | 1473 | 1474 | 1475 | 1476 | 1477 | 1478 | 1479 | 1480 | 1481 | 1482 | 1483 | 1484 | 1485 | 1486 | 1487 | 1488 | 1489 | 1490 | 1491 | 1492 | 1493 | 1494 | 1495 | 1496 | 1497 | 1498 | 1499 | 1500 | 1501 | 1502 | 1503 | 1504 | 1505 | 1506 | 1507 | 1508 | 1509 | 1510 | 1511 | 1512 | 1513 | 1514 | 1515 | 1516 | 1517 | 1518 | 1519 | 1520 | 1521 | 1522 | 1523 | 1524 | 1525 | 1526 | 1527 | 1528 | 1529 | 1530 | 1531 | 1532 | 1533 | 1534 | 1535 | 1536 | 1537 | 1538 | 1539 | 1540 | 1541 | 1542 | 1543 | 1544 | 1545 | 1546 | 1547 | 1548 | 1549 | 1550 | 1551 | 1552 | 1553 | 1554 | 1555 | 1556 | 1557 | 1558 | 1559 | 1560 | 1561 | 1562 | 1563 | 1564 | 1565 | 1566 | 1567 | 1568 | 1569 | 1570 | 1571 | 1572 | 1573 | 1574 | 1575 | 1576 | 1577 | 1578 | 1579 | 1580 | 1581 | 1582 | 1583 | 1584 | 1585 | 1586 | 1587 | 1588 | 1589 | 1590 | 1591 | 1592 | 1593 | 1594 | 1595 | 1596 | 1597 | 1598 | 1599 | 1600 | 1601 | 1602 | 1603 | 1604 | 1605 | 1606 | 1607 | 1608 | 1609 | 1610 | 1611 | 1612 | 1613 | 1614 | 1615 | 1616 | 1617 | 1618 | 1619 | 1620 | 1621 | 1622 | 1623 | 1624 | 1625 | 1626 | 1627 | 1628 | 1629 | 1630 | 1631 | 1632 | 1633 | 1634 | 1635 | 1636 | 1637 | 1638 | 1639 | 1640 | 1641 | 1642 | 1643 | 1644 | 1645 | 1646 | 1647 | 1648 | 1649 | 1650 | 1651 | 1652 | 1653 | 1654 | 1655 | 1656 | 1657 | 1658 | 1659 | 1660 | 1661 | 1662 | 1663 | 1664 | 1665 | 1666 | 1667 | 1668 | 1669 | 1670 | 1671 | 1672 | 1673 | 1674 | 1675 | 1676 | 1677 | 1678 | 1679 | 1680 | 1681 | 1682 | 1683 | 1684 | 1685 | 1686 | 1687 | 1688 | 1689 | 1690 | 1691 | 1692 | 1693 | 1694 | 1695 | 1696 | 1697 | 1698 | 1699 | 1700 | 1701 | 1702 | 1703 | 1704 | 1705 | 1706 | 1707 | 1708 | 1709 | 1710 | 1711 | 1712 | 1713 | 1714 | 1715 | 1716 | 1717 | 1718 | 1719 | 1720 | 1721 | 1722 | 1723 | 1724 | 1725 | 1726 | 1727 | 1728 | 1729 | 1730 | 1731 | 1732 | 1733 | 1734 | 1735 | 1736 | 1737 | 1738 | 1739 | 1740 | 1741 | 1742 | 1743 | 1744 | 1745 | 1746 | 1747 | 1748 | 1749 | 1750 | 1751 | 1752 | 1753 | 1754 | 1755 | 1756 | 1757 | 1758 | 1759 | 1760 | 1761 | 1762 | 1763 | 1764 | 1765 | 1766 | 1767 | 1768 | 1769 | 1770 | 1771 | 1772 | 1773 | 1774 | 1775 | 1776 | 1777 | 1778 | 1779 | 1780 | 1781 | 1782 | 1783 | 1784 | 1785 | 1786 | 1787 | 1788 | 1789 | 1790 | 1791 | 1792 | 1793 | 1794 | 1795 | 1796 | 1797 | 1798 | 1799 | 1800 | 1801 | 1802 | 1803 | 1804 | 1805 | 1806 | 1812 | 1813 | 1814 | 1815 | 1816 | 1817 | 1818 | 1819 | 1820 | 1821 | 1822 | 1 1823 | 1824 | 1825 | 1826 | 1827 | 1828 | 1829 | 1830 | 1831 | 1832 | 1833 | 1834 | 1835 | 1841 | 1842 | 1843 | 1844 | 1845 | 1846 | 1847 | 1848 | 1849 | 1850 | 1851 | 1 1852 | 1853 | 1854 | 1855 | 1856 | 1857 | 1858 | 1859 | 1860 | 1861 | 1862 | 1863 | 1864 | 1870 | 1871 | 1872 | 1873 | 1874 | 1875 | 1876 | 1877 | 1878 | 1879 | 1880 | 1 1881 | 1882 | 1883 | 1884 | 1885 | 1886 | 1887 | 1888 | 1889 | 1890 | 1891 | 1892 | 1893 | 1899 | 1900 | 1901 | 1902 | 1903 | 1904 | 1905 | 1906 | 1907 | 1908 | 1909 | 1 1910 | 1911 | 1912 | 1913 | 1914 | 1915 | 1916 | 1917 | 1918 | 1919 | 1920 | 1921 | 1922 | 1928 | 1929 | 1930 | 1931 | 1932 | 1933 | 1934 | 1935 | 1936 | 1937 | 1938 | 1 1939 | 1940 | 1941 | 1942 | 1943 | 1944 | 1945 | 1946 | 1947 | 1948 | 1949 | 1950 | 1951 | 1957 | 1958 | 1959 | 1960 | 1961 | 1962 | 1963 | 1964 | 1965 | 1966 | 1967 | 1 1968 | 1969 | 1970 | 1971 | 1972 | 1973 | 1974 | 1975 | 1976 | 1977 | 1978 | 1979 | 1980 | 1986 | 1987 | 1988 | 1989 | 1990 | 1991 | 1992 | 1993 | 1994 | 1995 | 1996 | 1 1997 | 1998 | 1999 | 2000 | 2001 | 2002 | 2003 | 2004 | 2005 | 2006 | 2007 | 2008 | 2009 | 2015 | 2016 | 2017 | 2018 | 2019 | 2020 | 2021 | 2022 | 2023 | 2024 | 2025 | 1 2026 | 2027 | 2028 | 2029 | 2030 | 2031 | 2032 | 2033 | 2034 | 2035 | 2036 | 2037 | 2038 | 2044 | 2045 | 2046 | 2047 | 2048 | 2049 | 2050 | 2051 | 2052 | 2053 | 2054 | 1 2055 | 2056 | 2057 | 2058 | 2059 | 2060 | 2061 | 2062 | 2063 | 2064 | 2065 | 2066 | 2067 | 2073 | 2074 | 2075 | 2076 | 2077 | 2078 | 2079 | 2080 | 2081 | 2082 | 2083 | 1 2084 | 2085 | 2086 | 2087 | 2088 | 2089 | 2090 | 2091 | 2092 | 2093 | 2094 | 2095 | 2096 | 2102 | 2103 | 2104 | 2105 | 2106 | 2107 | 2108 | 2109 | 2110 | 2111 | 2112 | 1 2113 | 2114 | 2115 | 2116 | 2117 | 2118 | 2119 | 2120 | 2121 | 2122 | 2123 | 2124 | 2125 | 2131 | 2132 | 2133 | 2134 | 2135 | 2136 | 2137 | 2138 | 2139 | 2140 | 2141 | 1 2142 | 2143 | 2144 | 2145 | 2146 | 2147 | 2148 | 2149 | 2150 | 2151 | 2152 | 2153 | 2154 | 2160 | 2161 | 2162 | 2163 | 2164 | 2165 | 2166 | 2167 | 2168 | 2169 | 2170 | 1 2171 | 2172 | 2173 | 2174 | 2175 | 2176 | 2177 | 2178 | 2179 | 2180 | 2181 | 2182 | 2183 | 2189 | 2190 | 2191 | 2192 | 2193 | 2194 | 2195 | 2196 | 2197 | 2198 | 2199 | 1 2200 | 2201 | 2202 | 2203 | 2204 | 2205 | 2206 | 2207 | 2208 | 2209 | 2210 | 2211 | 2212 | 2218 | 2219 | 2220 | 2221 | 2222 | 2223 | 2224 | 2225 | 2226 | 2227 | 2228 | 1 2229 | 2230 | 2231 | 2232 | 2233 | 2234 | 2235 | 2236 | 2237 | 2238 | 2239 | 2240 | 2241 | 2247 | 2248 | 2249 | 2250 | 2251 | 2252 | 2253 | 2254 | 2255 | 2256 | 2257 | 1 2258 | 2259 | 2260 | 2261 | --------------------------------------------------------------------------------