├── LICENSE ├── README.md ├── examples └── laser_tag │ ├── CMakeLists.txt │ ├── include │ ├── pomdp_planner │ │ ├── base │ │ │ ├── base_tag.h │ │ │ └── base_tag.h~ │ │ ├── laser_tag.h │ │ └── laser_tag_world.h │ └── robot_controller │ │ ├── laser_tag_controller.h │ │ └── youbot_interface.h │ ├── launch │ └── laser_tag.launch │ ├── package.xml │ ├── src │ ├── pomdp_planner │ │ ├── base │ │ │ └── base_tag.cpp │ │ ├── laser_tag.cpp │ │ ├── laser_tag_world.cpp │ │ └── main.cpp │ └── robot_controller │ │ ├── README.md │ │ ├── laser_tag_controller.cpp │ │ └── youbot_interface.cpp │ ├── srv │ ├── TagActionObs.srv │ └── YoubotActionObs.srv │ ├── test │ └── test_laser_tag_controller.cpp │ └── worlds │ └── 7x11_grid.world ├── images └── gazebo_screenshot.png └── robots └── youbot ├── youbot_description ├── CMakeLists.txt ├── README.md ├── controller │ └── ros_controller.urdf.xacro ├── meshes │ ├── sensors │ │ ├── asus_xtion_pro_live_base.dae │ │ ├── asus_xtion_pro_live_base.png │ │ ├── asus_xtion_pro_live_camera.dae │ │ ├── asus_xtion_pro_live_camera.png │ │ ├── hokuyo.dae │ │ ├── hokuyo_convex.dae │ │ ├── kinect.dae │ │ ├── kinect.tga │ │ └── microsoft_lifecam.dae │ ├── youbot_arm │ │ ├── arm0.dae │ │ ├── arm0_convex.dae │ │ ├── arm1.dae │ │ ├── arm1_convex.dae │ │ ├── arm2.dae │ │ ├── arm2_convex.dae │ │ ├── arm3.dae │ │ ├── arm3_convex.dae │ │ ├── arm4.dae │ │ ├── arm4_convex.dae │ │ ├── arm5.dae │ │ └── arm5_convex.dae │ ├── youbot_base │ │ ├── base.dae │ │ ├── base_convex.dae │ │ └── target.dae │ ├── youbot_gripper │ │ ├── finger.dae │ │ ├── finger_convex.dae │ │ ├── palm.dae │ │ └── palm_convex.dae │ └── youbot_plate │ │ ├── plate.dae │ │ └── plate_convex.dae ├── package.xml ├── robots │ ├── youbot.urdf.xacro │ ├── youbot_arm_only.urdf.xacro │ ├── youbot_base_laser.urdf.xacro │ ├── youbot_base_only.urdf.xacro │ ├── youbot_dual_arm.urdf.xacro │ └── youbot_target.urdf.xacro └── urdf │ ├── common.xacro │ ├── materials.urdf.xacro │ ├── misc │ ├── cam3d_tower.urdf.xacro │ └── sensor_angle.urdf.xacro │ ├── sensors │ ├── .svn │ │ ├── all-wcprops │ │ └── entries │ ├── asus_xtion_camera.gazebo.xacro │ ├── asus_xtion_camera.urdf.xacro │ ├── hokuyo_urg04_laser.gazebo.xacro │ ├── hokuyo_urg04_laser.urdf.xacro │ ├── hokuyo_urg04_laser_(perfect).gazebo.xacro │ ├── hokuyo_urg04_laser_(perfect).urdf.xacro │ ├── kinect_camera.gazebo.xacro │ ├── kinect_camera.urdf.xacro │ ├── microsoft_lifecam.gazebo.xacro │ └── microsoft_lifecam.urdf.xacro │ ├── youbot_arm │ ├── arm.gazebo.xacro │ ├── arm.transmission.xacro │ ├── arm.urdf.xacro │ └── limits.urdf.xacro │ ├── youbot_base │ ├── base.gazebo.xacro │ ├── base.transmission.xacro │ ├── base.urdf.xacro │ └── target_base.urdf.xacro │ ├── youbot_gripper │ ├── gripper.gazebo.xacro │ ├── gripper.transmission.xacro │ ├── gripper.urdf.xacro │ └── limits.urdf.xacro │ └── youbot_plate │ └── plate.urdf.xacro └── youbot_simulation ├── .gitignore ├── README.md ├── youbot_gazebo_control ├── CMakeLists.txt ├── config │ ├── arm_1_controller.yaml │ ├── arm_2_controller.yaml │ └── joint_state_controller.yaml ├── launch │ ├── arm_controller.launch │ └── joint_state_controller.launch └── package.xml ├── youbot_gazebo_robot ├── CMakeLists.txt ├── launch │ ├── youbot.launch │ ├── youbot_arm_only.launch │ ├── youbot_base_only.launch │ └── youbot_dual_arm.launch └── package.xml ├── youbot_gazebo_worlds ├── CMakeLists.txt ├── launch │ ├── empty_world.launch │ ├── robocup_at_work_2012.launch │ └── tower_of_hanoi.launch ├── package.xml └── urdf │ ├── robocup_at_work_2012.urdf │ └── tower_of_hanoi.urdf └── youbot_simulation ├── CMakeLists.txt └── package.xml /README.md: -------------------------------------------------------------------------------- 1 | # DESPOT Tutorials 2 | 3 | ![](images/gazebo_screenshot.png) 4 | 5 | In this tutorial, we present an example of using [DESPOT](https://github.com/AdaCompNUS/despot) with real-robots via ROS. We present a slightly modified version of the *Laser Tag* problem: a robot tries to find and tag a target which intentionally runs away inside an known environment. The robot is equiped with a noisy laser-range sensor to measure distances in eight directions. Initially, the robot is aware of neither the target's location nor its own location. In each step, the robot can move to the four adjacent positions. When the robot is adjacent to the target, it can call 'Tag' to terminate a successful pursuit. The simulation consists of two holonomic robots ([KUKA Youbot](http://www.youbot-store.com/)) inside a [Gazebo](http://gazebosim.org/) environment resembling the problem world described in DESPOT paper (Page 20) [1]. 6 | 7 | 8 | [1] N. Ye, A. Somani, D. Hsu, and W. Lee. [**DESPOT: Online POMDP planning with regularization**](http://bigbird.comp.nus.edu.sg/m2ap/wordpress/wp-content/uploads/2017/08/jair14.pdf). J. Artificial Intelligence Research, 58:231–266, 2017. 9 | 10 | [Copyright © 2014-2017 by National University of Singapore](http://motion.comp.nus.edu.sg/). 11 | 12 | ## Requirements 13 | 14 | Tested Operating Systems: 15 | 16 | | Ubuntu 14.04 17 | | :-------------: 18 | |[![Build Status](https://semaphoreapp.com/api/v1/projects/d4cca506-99be-44d2-b19e-176f36ec8cf1/128505/shields_badge.svg)](https://semaphoreapp.com/boennemann/badges) 19 | 20 | Dependencies: DESPOT, ROS Indigo+, Boost 1.55+, Gazebo 2+ 21 | 22 | ## Prerequisites 23 | 24 | Install [ROS Indigo](http://wiki.ros.org/indigo/Installation/Ubuntu). 25 | We recommend the `ros-indigo-desktop-full` version which includes Gazebo. 26 | 27 | Install the latest DESPOT using [CMakeLists](https://github.com/AdaCompNUS/despot#cmakelists). Make sure that DESPOT binaries and header files are installed. 28 | ```bash 29 | $ cd 30 | $ git checkout API_redesign # temporary, will be merged into master 31 | $ mkdir build; cd build 32 | 33 | $ cmake -DCMAKE_BUILD_TYPE=Release ../ 34 | $ make 35 | $ sudo make install 36 | ``` 37 | 38 | Install BOOST libraries with `sudo apt-get install libboost-all-dev` 39 | 40 | ## Installation 41 | 42 | If you haven't sourced your ROS environment, run: 43 | ```bash 44 | $ source /opt/ros/indigo/setup.bash OR /devel/setup.bash 45 | ``` 46 | 47 | Setup a fresh catkin workspace for **despot_tutorials**: 48 | 49 | ```bash 50 | $ mkdir -p ~/despot_ws/src 51 | $ cd ~/despot_ws/ 52 | $ catkin_make 53 | $ source devel/setup.bash 54 | ``` 55 | 56 | Clone the repository: 57 | ```bash 58 | $ cd ~/despot_ws/src 59 | $ git clone https://github.com/AdaCompNUS/despot_tutorials.git 60 | ``` 61 | 62 | Compile: 63 | ```bash 64 | cd ~/despot_ws 65 | catkin_make -DCMAKE_BUILD_TYPE=Release 66 | ``` 67 | 68 | ## Usage 69 | 70 | Launch the Gazebo environment and robot controllers: 71 | ```bash 72 | $ roslaunch laser_tag laser_tag.launch R1_noise:=0.5 73 | ``` 74 | 75 | On a separate terminal, run the POMDP planner: 76 | ```bash 77 | $ rosrun laser_tag pomdp_planner 78 | ``` 79 | 80 | You should see a 3D 7x11 grid world with two Youbots. The green robot should chase the red robot until 'Tag' is called. The `R1_noise` parameter specifies the gaussian noise (standard deviation in meters) of the green robot's laser range finder. 81 | 82 | ## Guidelines 83 | 84 | In general, to use DESPOT with real-world systems: 85 | 1. Define your POMDP model by inheriting the `DSPOMDP` class.
86 | *(See class `LaserTag` and its parent class `BaseTag` in [laser_tag.h](examples/laser_tag/include/pomdp_planner/laser_tag.h) and [base/base_tag.h](examples/laser_tag/include/pomdp_planner/base/base_tag.h).)* 87 | 2. Setup an interface to communicate with your systems by inheriting the `World` abstract class.
88 | *(See class `LaserTagWorld` in [laser_tag_world.h](examples/laser_tag/include/pomdp_planner/laser_tag_world.h) and [laser_tag_world.cpp](examples/laser_tag/src/pomdp_planner/laser_tag_world.cpp).)* 89 | * Implement the `Connect` and `Initialize` functions in `World` to estabilish connections with your system and intitialize it if possible. 90 | * Implement the `ExecuteAction` function in `World` to send actions to your system and receive observations from it in the formats specified in your POMDP model (e.g: `ACT_TYPE` & `OBS_TYPE` parameters). 91 | 3. Initialize your planner by inheirting the `Planner` class.
92 | *(See class `MyPlanner` in [main.cpp](examples/laser_tag/src/pomdp_planner/main.cpp).)* 93 | * Provide the planner your POMDP model and custom world by implementing the `InitializeModel` and `InitializeWorld` functions. 94 | * Choose "DESPOT" to be the solver by implementing `ChooseSolver`. 95 | * Setup default parameters, such as the number of scenarios, search time per step, etc., by implementing `InitializeDefaultParameters`. 96 | 4. Launch the planning pipeline in your main function by calling the `runPlanning` function in `Planner`.
97 | *(See the main function in [main.cpp](examples/laser_tag/src/pomdp_planner/main.cpp).)* 98 | 5. (Optional) Overwrite *Planner::PlanningLoop* to customize your planning pipeline, and overwrite *Planner::runStep* to customize the search-execute-update step inside the planning loop. 99 | *(See the PlanningLoop and runStep functions in [main.cpp](examples/laser_tag/src/pomdp_planner/main.cpp).)* 100 | 101 | -------------------------------------------------------------------------------- /examples/laser_tag/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_tag) 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 | geometry_msgs 12 | nav_msgs 13 | sensor_msgs 14 | tf 15 | message_generation 16 | ) 17 | 18 | find_package(Despot CONFIG REQUIRED) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | find_package(Boost REQUIRED COMPONENTS thread) 22 | 23 | ## Generate services in the 'srv' folder 24 | add_service_files( 25 | FILES 26 | YoubotActionObs.srv 27 | TagActionObs.srv 28 | ) 29 | 30 | ## Generate added messages and services with any dependencies listed here 31 | generate_messages( 32 | DEPENDENCIES 33 | std_msgs 34 | ) 35 | 36 | 37 | ################################### 38 | ## catkin specific configuration ## 39 | ################################### 40 | catkin_package( 41 | CATKIN_DEPENDS roscpp rospy std_msgs message_runtime 42 | ) 43 | 44 | ########### 45 | ## Build ## 46 | ########### 47 | 48 | include_directories( 49 | ${catkin_INCLUDE_DIRS} 50 | ${Boost_INCLUDE_DIRS} 51 | include/robot_controller 52 | include/pomdp_planner 53 | ) 54 | 55 | ## Declare a C++ executable 56 | add_executable(youbot_interface src/robot_controller/youbot_interface.cpp) 57 | add_dependencies(youbot_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 58 | target_link_libraries(youbot_interface ${catkin_LIBRARIES}) 59 | 60 | ## Declare a C++ executable 61 | add_executable(laser_tag_controller src/robot_controller/laser_tag_controller.cpp) 62 | add_dependencies(laser_tag_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 63 | target_link_libraries(laser_tag_controller ${catkin_LIBRARIES}) 64 | 65 | ## Declare a C++ executable 66 | add_executable(pomdp_planner src/pomdp_planner/base/base_tag.cpp src/pomdp_planner/laser_tag.cpp src/pomdp_planner/laser_tag_world.cpp src/pomdp_planner/main.cpp) 67 | add_dependencies(pomdp_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 68 | target_link_libraries(pomdp_planner despot ${catkin_LIBRARIES}) 69 | 70 | ############# 71 | ## Testing ## 72 | ############# 73 | add_executable(test_laser_tag_controller test/test_laser_tag_controller.cpp) 74 | add_dependencies(test_laser_tag_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 75 | target_link_libraries(test_laser_tag_controller ${catkin_LIBRARIES}) 76 | 77 | 78 | -------------------------------------------------------------------------------- /examples/laser_tag/include/pomdp_planner/base/base_tag.h: -------------------------------------------------------------------------------- 1 | #ifndef BASETAG_H 2 | #define BASETAG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace despot { 14 | 15 | /* ============================================================================== 16 | * TagState class 17 | * ==============================================================================*/ 18 | 19 | class TagState: public State { 20 | public: 21 | TagState(); 22 | TagState(int _state_id); 23 | 24 | std::string text() const; 25 | }; 26 | 27 | /* ============================================================================== 28 | * BaseTag class 29 | * ==============================================================================*/ 30 | 31 | class BaseTag: public MDP, 32 | public BeliefMDP, 33 | public StateIndexer, 34 | public StatePolicy, 35 | public MMAPInferencer { 36 | friend class TagState; 37 | friend class TagSHRPolicy; 38 | friend class TagSPParticleUpperBound; 39 | friend class TagManhattanUpperBound; 40 | friend class TagPOMCPPrior; 41 | friend class TagHistoryModePolicy; 42 | 43 | protected: 44 | static double TAG_REWARD; 45 | 46 | Floor floor_; 47 | 48 | std::vector states_; 49 | std::vector rob_; // rob_[s]: robot cell index for state s 50 | std::vector opp_; // opp_[s]: opponent cell index for state s 51 | 52 | std::vector > > transition_probabilities_; //state, action, [state, weight] 53 | OBS_TYPE same_loc_obs_; 54 | 55 | 56 | mutable MemoryPool memory_pool_; 57 | 58 | protected: 59 | std::map OppTransitionDistribution(int state) const; 60 | 61 | void ReadConfig(std::istream& is); 62 | void Init(std::istream& is); 63 | Coord MostLikelyOpponentPosition(const std::vector& particles) const; 64 | Coord MostLikelyRobPosition(const std::vector& particles) const; 65 | const TagState& MostLikelyState(const std::vector& particles) const; 66 | const State* GetMMAP(const std::vector& particles) const; 67 | void PrintTransitions() const; 68 | 69 | protected: 70 | std::string RandomMap(int height, int width, int obstacles); 71 | std::string BenchmarkMap(); 72 | int NextRobPosition(int rob, int opp, ACT_TYPE a) const; 73 | 74 | mutable std::vector default_action_; 75 | 76 | public: 77 | bool robot_pos_unknown_; 78 | static BaseTag* current_; 79 | 80 | BaseTag(); 81 | BaseTag(std::string params_file); 82 | virtual ~BaseTag(); 83 | 84 | bool Step(State& state, double random_num, ACT_TYPE action, 85 | double& reward) const; 86 | virtual bool Step(State& state, double random_num, ACT_TYPE action, 87 | double& reward, OBS_TYPE& obs) const = 0; 88 | inline int NumActions() const { 89 | return 5; 90 | } 91 | inline ACT_TYPE TagAction() const { 92 | return 4; 93 | } 94 | int NumStates() const; 95 | inline int GetIndex(const State* state) const { 96 | return state->state_id; 97 | } 98 | inline Coord GetRobPos(const State* state) const { 99 | return floor_.GetCell(rob_[state->state_id]); 100 | } 101 | inline int StateIndexToOppIndex(int index) const { 102 | return index % floor_.NumCells(); 103 | } 104 | inline int StateIndexToRobIndex(int index) const { 105 | return index / floor_.NumCells(); 106 | } 107 | inline int RobOppIndicesToStateIndex(int rob, int opp) const { 108 | return rob * floor_.NumCells() + opp; 109 | } 110 | inline const State* GetState(int index) const { 111 | return states_[index]; 112 | } 113 | 114 | virtual double ObsProb(OBS_TYPE obs, const State& state, ACT_TYPE action) const = 0; 115 | const std::vector& TransitionProbability(int s, ACT_TYPE a) const; 116 | double Reward(int s, ACT_TYPE a) const; 117 | 118 | State* CreateStartState(std::string type = "DEFAULT") const; 119 | virtual Belief* InitialBelief(const State* start, std::string type = "DEFAULT") const = 0; 120 | 121 | inline double GetMaxReward() const { 122 | return TAG_REWARD; 123 | } 124 | ParticleUpperBound* CreateParticleUpperBound(std::string name = "DEFAULT") const; 125 | ScenarioUpperBound* CreateScenarioUpperBound(std::string name = "DEFAULT", 126 | std::string particle_bound_name = "DEFAULT") const; 127 | BeliefUpperBound* CreateBeliefUpperBound(std::string name = "DEFAULT") const; 128 | 129 | inline ValuedAction GetBestAction() const { 130 | return ValuedAction(0, -1); 131 | } 132 | ScenarioLowerBound* CreateScenarioLowerBound(std::string name = "DEFAULT", 133 | std::string particle_bound_name = "DEFAULT") const; 134 | BeliefLowerBound* CreateBeliefLowerBound(std::string name = "DEFAULT") const; 135 | 136 | POMCPPrior* CreatePOMCPPrior(std::string name = "DEFAULT") const; 137 | 138 | void PrintState(const State& state, std::ostream& out = std::cout) const; 139 | void PrintBelief(const Belief& belief, std::ostream& out = std::cout) const; 140 | virtual void PrintObs(const State& state, OBS_TYPE obs, std::ostream& out = std::cout) const = 0; 141 | void PrintAction(ACT_TYPE action, std::ostream& out = std::cout) const; 142 | 143 | State* Allocate(int state_id, double weight) const; 144 | State* Copy(const State* particle) const; 145 | void Free(State* particle) const; 146 | int NumActiveParticles() const; 147 | 148 | const Floor& floor() const; 149 | 150 | int MostLikelyAction(const std::vector& particles) const; 151 | 152 | void ComputeDefaultActions(std::string type) const; 153 | int GetAction(const State& tagstate) const; 154 | 155 | Belief* Tau(const Belief* belief, ACT_TYPE action, OBS_TYPE obs) const; 156 | void Observe(const Belief* belief, ACT_TYPE action, std::map& obss) const = 0; 157 | double StepReward(const Belief* belief, ACT_TYPE action) const; 158 | }; 159 | 160 | /* ============================================================================== 161 | * TagBelief class 162 | * ==============================================================================*/ 163 | 164 | class TagBelief: public ParticleBelief { 165 | private: 166 | const BaseTag* tag_model_; 167 | public: 168 | TagBelief(std::vector particles, const BaseTag* model, Belief* prior = 169 | NULL); 170 | void Update(ACT_TYPE action, OBS_TYPE obs); 171 | }; 172 | 173 | } // namespace despot 174 | 175 | #endif 176 | -------------------------------------------------------------------------------- /examples/laser_tag/include/pomdp_planner/base/base_tag.h~: -------------------------------------------------------------------------------- 1 | #ifndef BASETAG_H 2 | #define BASETAG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace despot { 14 | 15 | /* ============================================================================== 16 | * TagState class 17 | * ==============================================================================*/ 18 | 19 | class TagState: public State { 20 | public: 21 | TagState(); 22 | TagState(int _state_id); 23 | 24 | std::string text() const; 25 | }; 26 | 27 | /* ============================================================================== 28 | * BaseTag class 29 | * ==============================================================================*/ 30 | 31 | class BaseTag: public MDP, 32 | public BeliefMDP, 33 | public StateIndexer, 34 | public StatePolicy, 35 | public MMAPInferencer { 36 | friend class TagState; 37 | friend class TagSHRPolicy; 38 | friend class TagSPParticleUpperBound; 39 | friend class TagManhattanUpperBound; 40 | friend class TagPOMCPPrior; 41 | friend class TagHistoryModePolicy; 42 | 43 | protected: 44 | static double TAG_REWARD; 45 | 46 | Floor floor_; 47 | 48 | std::vector states_; 49 | std::vector rob_; // rob_[s]: robot cell index for state s 50 | std::vector opp_; // opp_[s]: opponent cell index for state s 51 | 52 | std::vector > > transition_probabilities_; //state, action, [state, weight] 53 | OBS_TYPE same_loc_obs_; 54 | 55 | 56 | mutable MemoryPool memory_pool_; 57 | 58 | protected: 59 | std::map OppTransitionDistribution(int state) const; 60 | 61 | void ReadConfig(std::istream& is); 62 | void Init(std::istream& is); 63 | Coord MostLikelyOpponentPosition(const std::vector& particles) const; 64 | Coord MostLikelyRobPosition(const std::vector& particles) const; 65 | const TagState& MostLikelyState(const std::vector& particles) const; 66 | const State* GetMMAP(const std::vector& particles) const; 67 | void PrintTransitions() const; 68 | 69 | protected: 70 | std::string RandomMap(int height, int width, int obstacles); 71 | std::string BenchmarkMap(); 72 | int NextRobPosition(int rob, ACT_TYPE a) const; 73 | 74 | mutable std::vector default_action_; 75 | 76 | public: 77 | bool robot_pos_unknown_; 78 | static BaseTag* current_; 79 | 80 | BaseTag(); 81 | BaseTag(std::string params_file); 82 | virtual ~BaseTag(); 83 | 84 | bool Step(State& state, double random_num, ACT_TYPE action, 85 | double& reward) const; 86 | virtual bool Step(State& state, double random_num, ACT_TYPE action, 87 | double& reward, OBS_TYPE& obs) const = 0; 88 | inline int NumActions() const { 89 | return 5; 90 | } 91 | inline ACT_TYPE TagAction() const { 92 | return 4; 93 | } 94 | int NumStates() const; 95 | inline int GetIndex(const State* state) const { 96 | return state->state_id; 97 | } 98 | inline Coord GetRobPos(const State* state) const { 99 | return floor_.GetCell(rob_[state->state_id]); 100 | } 101 | inline int StateIndexToOppIndex(int index) const { 102 | return index % floor_.NumCells(); 103 | } 104 | inline int StateIndexToRobIndex(int index) const { 105 | return index / floor_.NumCells(); 106 | } 107 | inline int RobOppIndicesToStateIndex(int rob, int opp) const { 108 | return rob * floor_.NumCells() + opp; 109 | } 110 | inline const State* GetState(int index) const { 111 | return states_[index]; 112 | } 113 | 114 | virtual double ObsProb(OBS_TYPE obs, const State& state, ACT_TYPE action) const = 0; 115 | const std::vector& TransitionProbability(int s, ACT_TYPE a) const; 116 | double Reward(int s, ACT_TYPE a) const; 117 | 118 | State* CreateStartState(std::string type = "DEFAULT") const; 119 | virtual Belief* InitialBelief(const State* start, std::string type = "DEFAULT") const = 0; 120 | 121 | inline double GetMaxReward() const { 122 | return TAG_REWARD; 123 | } 124 | ParticleUpperBound* CreateParticleUpperBound(std::string name = "DEFAULT") const; 125 | ScenarioUpperBound* CreateScenarioUpperBound(std::string name = "DEFAULT", 126 | std::string particle_bound_name = "DEFAULT") const; 127 | BeliefUpperBound* CreateBeliefUpperBound(std::string name = "DEFAULT") const; 128 | 129 | inline ValuedAction GetBestAction() const { 130 | return ValuedAction(0, -1); 131 | } 132 | ScenarioLowerBound* CreateScenarioLowerBound(std::string name = "DEFAULT", 133 | std::string particle_bound_name = "DEFAULT") const; 134 | BeliefLowerBound* CreateBeliefLowerBound(std::string name = "DEFAULT") const; 135 | 136 | POMCPPrior* CreatePOMCPPrior(std::string name = "DEFAULT") const; 137 | 138 | void PrintState(const State& state, std::ostream& out = std::cout) const; 139 | void PrintBelief(const Belief& belief, std::ostream& out = std::cout) const; 140 | virtual void PrintObs(const State& state, OBS_TYPE obs, std::ostream& out = std::cout) const = 0; 141 | void PrintAction(ACT_TYPE action, std::ostream& out = std::cout) const; 142 | 143 | State* Allocate(int state_id, double weight) const; 144 | State* Copy(const State* particle) const; 145 | void Free(State* particle) const; 146 | int NumActiveParticles() const; 147 | 148 | const Floor& floor() const; 149 | 150 | int MostLikelyAction(const std::vector& particles) const; 151 | 152 | void ComputeDefaultActions(std::string type) const; 153 | int GetAction(const State& tagstate) const; 154 | 155 | Belief* Tau(const Belief* belief, ACT_TYPE action, OBS_TYPE obs) const; 156 | void Observe(const Belief* belief, ACT_TYPE action, std::map& obss) const = 0; 157 | double StepReward(const Belief* belief, ACT_TYPE action) const; 158 | }; 159 | 160 | /* ============================================================================== 161 | * TagBelief class 162 | * ==============================================================================*/ 163 | 164 | class TagBelief: public ParticleBelief { 165 | private: 166 | const BaseTag* tag_model_; 167 | public: 168 | TagBelief(std::vector particles, const BaseTag* model, Belief* prior = 169 | NULL); 170 | void Update(ACT_TYPE action, OBS_TYPE obs); 171 | }; 172 | 173 | } // namespace despot 174 | 175 | #endif 176 | -------------------------------------------------------------------------------- /examples/laser_tag/include/pomdp_planner/laser_tag.h: -------------------------------------------------------------------------------- 1 | #ifndef LASERTAG_H 2 | #define LASERTAG_H 3 | 4 | #include "base/base_tag.h" 5 | 6 | /* ============================================================================= 7 | * LaserTag class 8 | * =============================================================================*/ 9 | 10 | namespace despot { 11 | 12 | class LaserTag: public BaseTag { 13 | private: 14 | static int NBEAMS; 15 | static int BITS_PER_READING; 16 | 17 | double noise_sigma_; 18 | double unit_size_; 19 | std::vector > > reading_distributions_; 20 | 21 | public: 22 | LaserTag(); 23 | LaserTag(std::string params_file); 24 | LaserTag(std::string params_file, double noise_sigma); 25 | double LaserRange(const State& state, int dir) const; 26 | void Init(); 27 | int GetReading(int); 28 | void NoiseSigma(double noise_sigma); 29 | 30 | bool Step(State& state, double random_num, ACT_TYPE action, double& reward) const; 31 | bool Step(State& state, double random_num, ACT_TYPE action, 32 | double& reward, OBS_TYPE& obs) const; 33 | double ObsProb(OBS_TYPE obs, const State& state, ACT_TYPE action) const; 34 | 35 | Belief* InitialBelief(const State* start, std::string type = "DEFAULT") const; 36 | 37 | void PrintObs(const State& state, OBS_TYPE obs, std::ostream& out = std::cout) const; 38 | 39 | static int GetReading(OBS_TYPE obs, OBS_TYPE dir); 40 | static void SetReading(OBS_TYPE& obs, OBS_TYPE reading, OBS_TYPE dir); 41 | int GetBucket(double noisy) const; 42 | 43 | void Observe(const Belief* belief, ACT_TYPE action, std::map& obss) const; 44 | 45 | friend std::ostream& operator<<(std::ostream& os, const LaserTag& lasertag); 46 | }; 47 | 48 | } // namespace despot 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /examples/laser_tag/include/pomdp_planner/laser_tag_world.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | using namespace despot; 5 | 6 | class LaserTagWorld: public World { 7 | public: 8 | ros::NodeHandlePtr nh; 9 | ros::ServiceClient client; 10 | 11 | virtual bool Connect(); 12 | 13 | //Initialize or reset the environment (for simulators or POMDP world only), return the start state of the system if applicable 14 | virtual State* Initialize(); 15 | 16 | //Get the state of the system (only applicable for simulators or POMDP world) 17 | virtual State* GetCurrentState(); 18 | 19 | //Send action to be executed by the system, receive observations terminal signals from the system 20 | virtual bool ExecuteAction(ACT_TYPE action, OBS_TYPE& obs); 21 | 22 | static double noise_sigma_; 23 | }; 24 | -------------------------------------------------------------------------------- /examples/laser_tag/include/robot_controller/laser_tag_controller.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class LaserTagController 15 | { 16 | public: 17 | LaserTagController(); 18 | enum DIRECTION 19 | { 20 | NORTH = 0, 21 | EAST = 1, 22 | SOUTH = 2, 23 | WEST = 3, 24 | STAY = 4, 25 | }; 26 | 27 | private: 28 | ros::NodeHandle nh_; 29 | 30 | ros::ServiceClient robot_client_; 31 | ros::ServiceClient target_client_; 32 | ros::Subscriber robot_pose_sub_; 33 | ros::Subscriber target_pose_sub_; 34 | 35 | ros::Subscriber target_laser_sub_; 36 | ros::ServiceServer control_srv_; 37 | 38 | std::vector target_laser_readings_; 39 | std::vector robot_obs_, target_obs_; 40 | std::vector robot_position_, target_position_; 41 | 42 | void laser_cb(const sensor_msgs::LaserScan::ConstPtr& scan); 43 | void target_pose_cb(const nav_msgs::Odometry::ConstPtr& odom); 44 | void robot_pose_cb(const nav_msgs::Odometry::ConstPtr& odom); 45 | 46 | bool LaserTagActionObs(laser_tag::TagActionObs::Request &req, laser_tag::TagActionObs::Response &res); 47 | 48 | DIRECTION TargetNextAction(void); 49 | bool CheckCollision(DIRECTION); 50 | void RobotSrvCall(int action); 51 | void TargetSrvCall(DIRECTION action); 52 | 53 | std::string ActionToString(int action); 54 | }; 55 | -------------------------------------------------------------------------------- /examples/laser_tag/include/robot_controller/youbot_interface.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | //#include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | class YoubotInterface 20 | { 21 | public: 22 | enum DIRECTION 23 | { 24 | NORTH = 0, 25 | EAST = 1, 26 | SOUTH = 2, 27 | WEST = 3, 28 | NORTHEAST = 4, 29 | SOUTHEAST = 5, 30 | SOUTHWEST = 6, 31 | NORTHWEST = 7 32 | }; 33 | 34 | YoubotInterface(); 35 | void Goto(float x, float y); 36 | 37 | 38 | private: 39 | 40 | ros::NodeHandlePtr nh_; 41 | 42 | ros::Subscriber base_pose_sub_; 43 | ros::Subscriber laser_sub_; 44 | 45 | geometry_msgs::Pose base_pose_; 46 | std::vector laser_readings_; 47 | 48 | ros::ServiceServer control_srv_; 49 | ros::Publisher vel_pub_; 50 | 51 | float noise_sigma_; 52 | 53 | void base_pose_cb(const nav_msgs::Odometry::ConstPtr& odom); 54 | void laser_cb(const sensor_msgs::LaserScan::ConstPtr& scan); 55 | geometry_msgs::Vector3 quat2euler(const geometry_msgs::Quaternion quat); 56 | 57 | bool DiscreteController(laser_tag::YoubotActionObs::Request &req, 58 | laser_tag::YoubotActionObs::Response &res); 59 | 60 | }; 61 | -------------------------------------------------------------------------------- /examples/laser_tag/launch/laser_tag.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /examples/laser_tag/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_tag 4 | 0.0.0 5 | DESPOT Tutorial - Laser Tag POMDP Problem 6 | 7 | 8 | AdacompNUS 9 | GPLv2 10 | 11 | catkin 12 | roscpp 13 | rospy 14 | std_msgs 15 | geometry_msgs 16 | nav_msgs 17 | sensor_msgs 18 | tf 19 | message_generation 20 | 21 | roscpp 22 | rospy 23 | std_msgs 24 | geometry_msgs 25 | message_runtime 26 | nav_msgs 27 | sensor_msgs 28 | tf 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /examples/laser_tag/src/pomdp_planner/laser_tag.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | namespace despot { 8 | 9 | const OBS_TYPE ONE = 1; 10 | int LaserTag::NBEAMS = 8; 11 | int LaserTag::BITS_PER_READING = 7; 12 | 13 | /* ============================================================================= 14 | * LaserTag class 15 | * =============================================================================*/ 16 | 17 | LaserTag::LaserTag() : 18 | BaseTag(), 19 | noise_sigma_(0.5), 20 | unit_size_(1.0) { 21 | istringstream iss(BenchmarkMap()); 22 | BaseTag::Init(iss); 23 | Init(); 24 | robot_pos_unknown_ = false; 25 | } 26 | 27 | LaserTag::LaserTag(string params_file) : 28 | BaseTag(params_file), 29 | noise_sigma_(0.5), 30 | unit_size_(1.0) { 31 | Init(); 32 | robot_pos_unknown_ = false; 33 | } 34 | 35 | LaserTag::LaserTag(string params_file, double noise_sigma) : 36 | BaseTag(params_file), 37 | unit_size_(1.0) { 38 | Init(); 39 | robot_pos_unknown_ = false; 40 | noise_sigma_ = noise_sigma; 41 | } 42 | 43 | double LaserTag::LaserRange(const State& state, int dir) const { 44 | Coord rob = floor_.GetCell(rob_[state.state_id]), opp = floor_.GetCell( 45 | opp_[state.state_id]); 46 | int d = 1; 47 | while (true) { 48 | Coord coord = rob + Compass::DIRECTIONS[dir] * d; 49 | if (floor_.GetIndex(coord) == -1 || coord == opp) 50 | break; 51 | d++; 52 | } 53 | int x = Compass::DIRECTIONS[dir].x, y = Compass::DIRECTIONS[dir].y; 54 | 55 | return d * sqrt(x * x + y * y); 56 | } 57 | 58 | void LaserTag::Init() { 59 | for (int i = 0; i < NBEAMS; i++) 60 | SetReading(same_loc_obs_, 101, i); 61 | 62 | reading_distributions_.resize(NumStates()); 63 | 64 | for (int s = 0; s < NumStates(); s++) { 65 | reading_distributions_[s].resize(NBEAMS); 66 | 67 | for (int d = 0; d < NBEAMS; d++) { 68 | double dist = LaserRange(*states_[s], d); 69 | for (int reading = 0; reading < dist / unit_size_; reading++) { 70 | double min_noise = reading * unit_size_ - dist; 71 | double max_noise = min(dist, (reading + 1) * unit_size_) - dist; 72 | double prob = 73 | 2 74 | * (gausscdf(max_noise, 0, noise_sigma_) 75 | - (reading > 0 ? 76 | gausscdf(min_noise, 0, noise_sigma_) : 0 /*min_noise = -infty*/)); 77 | 78 | reading_distributions_[s][d].push_back(prob); 79 | } 80 | } 81 | } 82 | } 83 | 84 | bool LaserTag::Step(State& state, double random_num, ACT_TYPE action, 85 | double& reward) const { 86 | Random random(random_num); 87 | bool terminal = BaseTag::Step(state, random.NextDouble(), action, reward); 88 | return terminal; 89 | } 90 | 91 | bool LaserTag::Step(State& state, double random_num, ACT_TYPE action, double& reward, 92 | OBS_TYPE& obs) const { 93 | Random random(random_num); 94 | bool terminal = BaseTag::Step(state, random.NextDouble(), action, reward); 95 | 96 | if (terminal) { 97 | obs = same_loc_obs_; 98 | } else { 99 | if (rob_[state.state_id] == opp_[state.state_id]) 100 | obs = same_loc_obs_; 101 | else { 102 | const vector >& distribution = reading_distributions_[state.state_id]; 103 | 104 | obs = 0; 105 | for (int dir = 0; dir < NBEAMS; dir++) { 106 | double mass = random.NextDouble(); 107 | int reading = 0; 108 | for (; reading < distribution[dir].size(); reading++) { 109 | mass -= distribution[dir][reading]; 110 | if (mass < Globals::TINY) 111 | break; 112 | } 113 | SetReading(obs, reading, dir); 114 | } 115 | } 116 | } 117 | 118 | return terminal; 119 | } 120 | 121 | double LaserTag::ObsProb(OBS_TYPE obs, const State& state, ACT_TYPE action) const { 122 | if (rob_[state.state_id] == opp_[state.state_id]) 123 | return obs == same_loc_obs_; 124 | 125 | double prod = 1.0; 126 | for (int dir = 0; dir < NBEAMS; dir++) { 127 | int reading = GetReading(obs, dir); 128 | if (reading >= LaserRange(state, dir) / unit_size_) 129 | return 0; 130 | double prob_mass = reading_distributions_[state.state_id][dir][reading]; 131 | prod *= prob_mass; 132 | } 133 | 134 | return prod; 135 | } 136 | 137 | void LaserTag::PrintObs(const State& state, OBS_TYPE obs, ostream& out) const { 138 | for (int i = 0; i < NBEAMS; i++) 139 | out << GetReading(obs, i) << " "; 140 | out << endl; 141 | } 142 | 143 | int LaserTag::GetReading(OBS_TYPE obs, OBS_TYPE dir) { 144 | return (obs >> (dir * BITS_PER_READING)) & ((ONE << BITS_PER_READING) - 1); 145 | } 146 | 147 | void LaserTag::SetReading(OBS_TYPE& obs, OBS_TYPE reading, OBS_TYPE dir) { 148 | // Clear bits 149 | obs &= ~(((ONE << BITS_PER_READING) - 1) << (dir * BITS_PER_READING)); 150 | // Set bits 151 | obs |= reading << (dir * BITS_PER_READING); 152 | } 153 | 154 | int LaserTag::GetBucket(double noisy) const { 155 | return (int) std::floor(noisy / unit_size_); 156 | } 157 | 158 | Belief* LaserTag::InitialBelief(const State* start, string type) const { 159 | //assert(start != NULL); 160 | 161 | vector particles; 162 | int N = floor_.NumCells(); 163 | double wgt = 1.0 / N / N; 164 | for (int rob = 0; rob < N; rob++) { 165 | for (int opp = 0; opp < N; opp++) { 166 | TagState* state = static_cast(Allocate( 167 | RobOppIndicesToStateIndex(rob, opp), wgt)); 168 | particles.push_back(state); 169 | } 170 | } 171 | 172 | ParticleBelief* belief = new ParticleBelief(particles, this); 173 | belief->state_indexer(this); 174 | return belief; 175 | } 176 | 177 | ostream& operator<<(ostream& os, const LaserTag& lasertag) { 178 | for (int s = 0; s < lasertag.NumStates(); s++) { 179 | os << "State " << s << " " << lasertag.opp_[s] << " " 180 | << lasertag.rob_[s] << " " << lasertag.floor_.NumCells() << endl; 181 | lasertag.PrintState(*lasertag.states_[s], os); 182 | 183 | for (int d = 0; d < lasertag.NBEAMS; d++) { 184 | os << d; 185 | for (int i = 0; i < lasertag.reading_distributions_[s][d].size(); i++) 186 | os << " " << lasertag.reading_distributions_[s][d][i]; 187 | os << endl; 188 | } 189 | } 190 | return os; 191 | } 192 | 193 | void LaserTag::Observe(const Belief* belief, ACT_TYPE action, 194 | map& obss) const { 195 | cerr << "Exit: Two many observations!" << endl; 196 | exit(0); 197 | } 198 | 199 | void LaserTag::NoiseSigma(double noise_sigma) 200 | { 201 | noise_sigma_ = noise_sigma; 202 | } 203 | 204 | } // namespace despot 205 | -------------------------------------------------------------------------------- /examples/laser_tag/src/pomdp_planner/laser_tag_world.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // for tests 7 | #include 8 | 9 | using namespace despot; 10 | 11 | #define TAG 4 12 | #define DEFAULT_NOISE_SIGMA 0.5 13 | #define TERMINATION_OBSERVATION 101 14 | #define NUM_LASER_DIRECTIONS 8 15 | 16 | double LaserTagWorld::noise_sigma_ = DEFAULT_NOISE_SIGMA; 17 | 18 | bool LaserTagWorld::Connect(){ 19 | 20 | // initialize ROS node 21 | int argc; 22 | char ** argv; 23 | ros::init(argc, argv, "test_laser_tag"); 24 | nh = ros::NodeHandlePtr(new ros::NodeHandle); 25 | 26 | // get laser's noise sigma 27 | if (nh->getParam("/robot/youbot_interface/noise_sigma", noise_sigma_)) 28 | { 29 | ROS_INFO("Initialized laser with noise: %f stddev.", noise_sigma_); 30 | } 31 | else 32 | { 33 | ROS_INFO("Initialized laser with default noise: %f stddev.", noise_sigma_); 34 | } 35 | 36 | // wait for laser tag controller service to show up (blocking call) 37 | ros::service::waitForService("laser_tag_action_obs", -1); 38 | 39 | // setup service client 40 | client = nh->serviceClient("laser_tag_action_obs"); 41 | } 42 | 43 | //Initialize or reset the environment (for simulators or POMDP world only), return the start state of the system if applicable 44 | State* LaserTagWorld::Initialize(){ 45 | return NULL; 46 | } 47 | 48 | //Get the state of the system (only applicable for simulators or POMDP world) 49 | State* LaserTagWorld::GetCurrentState(){ 50 | return NULL; 51 | } 52 | 53 | //Send action to be executed by the system, receive observations terminal signals from the system 54 | bool LaserTagWorld::ExecuteAction(ACT_TYPE action, OBS_TYPE& obs){ 55 | 56 | /* laser_tag::TagActionObs is a ROS service that takes in an action (integer) 57 | * and outputs observations (8 intergers) after executing the action. If the 58 | * 'Tag' action is called, it returns a boolean 'tag_success' with the outcome. 59 | */ 60 | 61 | laser_tag::TagActionObs srv; 62 | srv.request.action = (int) action; // actions: 0 - North, 1 - East, 2 - South, 3 - West, 4 - Tag 63 | if (client.call(srv)) 64 | { 65 | 66 | // successful tag 67 | if(action == TAG && srv.response.tag_success == true) 68 | { 69 | obs=(OBS_TYPE)0; 70 | for (int dir = 0; dir < NUM_LASER_DIRECTIONS; dir++) { 71 | LaserTag::SetReading(obs, TERMINATION_OBSERVATION, dir); 72 | } 73 | return 1; // exit 74 | } 75 | // continue to observe 76 | else 77 | { 78 | // observations after executing action 79 | std::vector laser_obs = srv.response.observations; 80 | 81 | // print observations 82 | ROS_INFO("Laser Observations"); 83 | ROS_INFO("North: %d" , laser_obs[0]); 84 | ROS_INFO("East: %d" , laser_obs[1]); 85 | ROS_INFO("South: %d" , laser_obs[2]); 86 | ROS_INFO("West: %d" , laser_obs[3]); 87 | ROS_INFO("NorthEast: %d", laser_obs[4]); 88 | ROS_INFO("SouthEast: %d", laser_obs[5]); 89 | ROS_INFO("SouthWest: %d", laser_obs[6]); 90 | ROS_INFO("NorthWest: %d", laser_obs[7]); 91 | 92 | for (int dir = 0; dir < 8; dir++) { 93 | LaserTag::SetReading(obs, laser_obs[dir], dir); 94 | } 95 | return 0; // continue 96 | } 97 | } 98 | else 99 | { 100 | ROS_ERROR("Failed to execute action & receive observations. Something went wrong with the robot controller!"); 101 | return 0; // continue 102 | } 103 | } 104 | 105 | -------------------------------------------------------------------------------- /examples/laser_tag/src/pomdp_planner/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace despot; 6 | 7 | class MyPlanner: public Planner { 8 | public: 9 | MyPlanner() { 10 | } 11 | 12 | DSPOMDP* InitializeModel(option::Option* options) { 13 | DSPOMDP* model = !options[E_PARAMS_FILE] ? 14 | new LaserTag() : new LaserTag(options[E_PARAMS_FILE].arg); 15 | return model; 16 | } 17 | 18 | World* InitializeWorld(std::string& world_type, DSPOMDP* model, option::Option* options) 19 | { 20 | //Create a custom world as defined and implemented by the user 21 | LaserTagWorld* world = new LaserTagWorld(); 22 | //Establish connection with external system 23 | world->Connect(); 24 | //Initialize the state of the external system 25 | world->Initialize(); 26 | static_cast(model)->NoiseSigma(LaserTagWorld::noise_sigma_); 27 | //Inform despot the type of world 28 | world_type = "simulator"; 29 | return world; 30 | } 31 | 32 | void InitializeDefaultParameters() { 33 | Globals::config.pruning_constant = 0.01; 34 | } 35 | 36 | std::string ChooseSolver(){ 37 | return "DESPOT"; 38 | } 39 | 40 | /*Customize your planning pipeline by overloading the following function if necessary*/ 41 | void PlanningLoop(Solver*& solver, World* world, Logger* logger) { 42 | for (int i = 0; i < Globals::config.sim_len; i++) { 43 | bool terminal = RunStep(solver, world, logger); 44 | if (terminal) 45 | break; 46 | } 47 | } 48 | 49 | /*Customize the inner step of the planning pipeline by overloading the following function if necessary*/ 50 | bool RunStep(Solver* solver, World* world, Logger* logger) { 51 | logger->CheckTargetTime(); 52 | 53 | double step_start_t = get_time_second(); 54 | 55 | double start_t = get_time_second(); 56 | ACT_TYPE action = solver->Search().action; 57 | double end_t = get_time_second(); 58 | double search_time = (end_t - start_t); 59 | logi << "[Custom RunStep] Time spent in " << typeid(*solver).name() 60 | << "::Search(): " << search_time << endl; 61 | 62 | OBS_TYPE obs; 63 | start_t = get_time_second(); 64 | bool terminal = world->ExecuteAction(action, obs); 65 | end_t = get_time_second(); 66 | double execute_time = (end_t - start_t); 67 | logi << "[Custom RunStep] Time spent in ExecuteAction(): " << execute_time << endl; 68 | 69 | start_t = get_time_second(); 70 | solver->BeliefUpdate(action, obs); 71 | end_t = get_time_second(); 72 | double update_time = (end_t - start_t); 73 | logi << "[Custom RunStep] Time spent in Update(): " << update_time << endl; 74 | 75 | return logger->SummarizeStep(step_++, round_, terminal, action, obs, 76 | step_start_t); 77 | } 78 | 79 | }; 80 | 81 | int main(int argc, char* argv[]) { 82 | return MyPlanner().RunPlanning(argc, argv); 83 | } 84 | -------------------------------------------------------------------------------- /examples/laser_tag/src/robot_controller/README.md: -------------------------------------------------------------------------------- 1 | # Robot Controller 2 | 3 | A high-level interface to send actions to the robot, and receive obseravtions. 4 | 5 | #### Test Controllers 6 | Run a test to see if you can control the robots: 7 | ```bash 8 | $ rosrun laser_tag test_laser_tag_controller 9 | ``` 10 | The first robot (bottom left) should move North. And the other robot should try and run away. After executing the action, you should see a print-out of the laser-readings: 11 | 12 | ``` 13 | [ INFO] [1507790509.778345756, 1131.223000000]: Laser Observations 14 | [ INFO] [1507790509.778420017, 1131.223000000]: North: 0 15 | [ INFO] [1507790509.778456571, 1131.223000000]: East: 7 16 | [ INFO] [1507790509.778491221, 1131.223000000]: South: 2 17 | [ INFO] [1507790509.778523052, 1131.223000000]: West: 3 18 | [ INFO] [1507790509.778550735, 1131.223000000]: NorthEast: 4 19 | [ INFO] [1507790509.778578519, 1131.223000000]: SouthEast: 4 20 | [ INFO] [1507790509.778604918, 1131.223000000]: SouthWest: 4 21 | [ INFO] [1507790509.778631267, 1131.223000000]: NorthWest: 5 22 | ``` 23 | 24 | See [test_laser_tag_controller.cpp](examples/laser_tag/test/test_laser_tag_controller.cpp) for details on giving an action to the robot, and receiving observations. 25 | 26 | #### Command-Line Interface 27 | You can also send actions to the robot directly using rosservice 28 | ```bash 29 | $ rosservice call /laser_tag_action_obs "action: 1" 30 | ``` 31 | -------------------------------------------------------------------------------- /examples/laser_tag/src/robot_controller/laser_tag_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "laser_tag_controller.h" 2 | 3 | #define CONTROL_RATE 30 4 | #define RANDOM_SEED 42 5 | 6 | LaserTagController::LaserTagController() 7 | { 8 | target_laser_readings_.resize(4); // North, East, South, West 9 | robot_position_.resize(2); // x, y 10 | target_position_.resize(2); 11 | 12 | robot_client_ = nh_.serviceClient("/robot/youbot_discrete_controller"); 13 | target_client_ = nh_.serviceClient("/target/youbot_discrete_controller"); 14 | 15 | target_laser_sub_ = nh_.subscribe("/target/laser_scan", 30, &LaserTagController::laser_cb, this); 16 | robot_pose_sub_ = nh_.subscribe("/robot/odom", 30, &LaserTagController::robot_pose_cb, this); 17 | target_pose_sub_ = nh_.subscribe("/target/odom", 30, &LaserTagController::target_pose_cb, this); 18 | 19 | control_srv_ = nh_.advertiseService("laser_tag_action_obs", &LaserTagController::LaserTagActionObs, this); 20 | 21 | srand(RANDOM_SEED); 22 | } 23 | 24 | void LaserTagController::laser_cb(const sensor_msgs::LaserScan::ConstPtr& scan) 25 | { 26 | // raw target sensor readings for collision avoidance during evasion 27 | target_laser_readings_[SOUTH] = scan->ranges[0]; 28 | target_laser_readings_[EAST] = scan->ranges[2]; 29 | target_laser_readings_[NORTH] = scan->ranges[4]; 30 | target_laser_readings_[WEST] = scan->ranges[6]; 31 | } 32 | 33 | void LaserTagController::robot_pose_cb(const nav_msgs::Odometry::ConstPtr& odom) 34 | { 35 | robot_position_[0] = (int) floor(odom->pose.pose.position.x); 36 | robot_position_[1] = (int) floor(odom->pose.pose.position.y); 37 | } 38 | 39 | void LaserTagController::target_pose_cb(const nav_msgs::Odometry::ConstPtr& odom) 40 | { 41 | target_position_[0] = (int) floor(odom->pose.pose.position.x); 42 | target_position_[1] = (int) floor(odom->pose.pose.position.y); 43 | } 44 | 45 | std::string LaserTagController::ActionToString(int action) 46 | { 47 | if (action == NORTH) 48 | return "North"; 49 | else if (action == EAST) 50 | return "East"; 51 | else if (action == SOUTH) 52 | return "South"; 53 | else if (action == WEST) 54 | return "West"; 55 | else if (action == STAY) 56 | return "Stay"; 57 | } 58 | 59 | bool LaserTagController::CheckCollision(LaserTagController::DIRECTION direction) 60 | { 61 | return (target_laser_readings_[direction] < 1.0f); 62 | } 63 | 64 | LaserTagController::DIRECTION LaserTagController::TargetNextAction(void) 65 | { 66 | /* 67 | * Evasive maneuver 68 | */ 69 | 70 | 71 | // generate random float between 0 & 1 72 | double r = ((double) rand() / (RAND_MAX)); 73 | 74 | int delta_x = target_position_[0] - robot_position_[0]; 75 | int delta_y = target_position_[1] - robot_position_[1]; 76 | 77 | if (r >= 0.0f && r <= 0.4f) 78 | { 79 | if (delta_x == 0) // target is directly below robot 80 | { 81 | if (r >= 0.0f && r <= 0.2f) 82 | return EAST; 83 | else 84 | return WEST; 85 | } 86 | else if (delta_x < 0) // target is to the right of the robot 87 | { 88 | return WEST; 89 | } 90 | else if (delta_x > 0) // target is to the left of the robot 91 | { 92 | return EAST; 93 | } 94 | } 95 | 96 | else if (r > 0.4f && r <= 0.8f) 97 | { 98 | if (delta_y == 0) // target is directly next to the robot 99 | { 100 | if (r > 0.4f && r <= 0.6f) 101 | return NORTH; 102 | else 103 | return SOUTH; 104 | } 105 | else if (delta_y < 0) // target is above the robot 106 | { 107 | return SOUTH; 108 | } 109 | else if (delta_y > 0) // target is below the robot 110 | { 111 | return NORTH; 112 | } 113 | } 114 | 115 | return STAY; 116 | } 117 | 118 | 119 | void LaserTagController::RobotSrvCall(int action) 120 | { 121 | laser_tag::YoubotActionObs srv; 122 | srv.request.direction = this->ActionToString(action); 123 | 124 | if (robot_client_.call(srv)) 125 | ROS_INFO("Finished executing robot action"); 126 | else 127 | ROS_ERROR("Something went wrong with the robot interface"); 128 | 129 | robot_obs_ = srv.response.laser_readings; 130 | } 131 | 132 | 133 | void LaserTagController::TargetSrvCall(LaserTagController::DIRECTION action) 134 | { 135 | laser_tag::YoubotActionObs srv; 136 | srv.request.direction = this->ActionToString(action); 137 | 138 | if (target_client_.call(srv)) 139 | ROS_INFO("Finished executing target action"); 140 | else 141 | ROS_ERROR("Something went wrong with the target interface"); 142 | 143 | target_obs_ = srv.response.laser_readings; 144 | } 145 | 146 | 147 | bool LaserTagController::LaserTagActionObs(laser_tag::TagActionObs::Request &req, 148 | laser_tag::TagActionObs::Response &res) 149 | { 150 | // default tag status 151 | res.tag_success = false; 152 | 153 | // check if action is valid 154 | if (req.action < 0 || req.action > 4) 155 | { 156 | ROS_ERROR("Invalid Action Integer. Valid actions (0-4): 0 - North, 1 - East, 2 - South, 3 - West, 4 - Tag"); 157 | return false; 158 | } 159 | 160 | // plan evasive action for target 161 | LaserTagController::DIRECTION next_target_action = this->TargetNextAction(); 162 | next_target_action = (this->CheckCollision(next_target_action) || req.action == 4) ? STAY : next_target_action; 163 | 164 | boost::thread thread_target, thread_robot; 165 | 166 | // send commands to robot and target 167 | if (next_target_action != STAY) 168 | thread_target = boost::thread(boost::bind(&LaserTagController::TargetSrvCall, this, next_target_action)); 169 | thread_robot = boost::thread(boost::bind(&LaserTagController::RobotSrvCall, this, req.action)); 170 | 171 | // wait for robot and target to finish 172 | if (next_target_action != STAY) 173 | thread_target.join(); 174 | thread_robot.join(); 175 | 176 | // latest observations 177 | res.observations = robot_obs_; 178 | 179 | // check if Tag 180 | if (req.action == 4) 181 | { 182 | int delta_x = target_position_[0] - robot_position_[0]; 183 | int delta_y = target_position_[1] - robot_position_[1]; 184 | 185 | // target has to be in one of the four directions 186 | if ( (abs(delta_x) + abs(delta_y)) == 1) 187 | { 188 | ROS_INFO("!!!!!!!!!!!!!!!TAG!!!!!!!!!!!!!!!!!!!!"); 189 | res.tag_success = true; 190 | } 191 | else 192 | { 193 | ROS_WARN("Invalid Tag!"); 194 | } 195 | } 196 | 197 | return true; 198 | } 199 | 200 | int main(int argc, char** argv) 201 | { 202 | ros::init(argc, argv, "laser_tag_controller"); 203 | LaserTagController youbot_interface; 204 | 205 | // Main Loop 206 | ros::Rate loop_rate(CONTROL_RATE); 207 | while (true) 208 | { 209 | ros::spinOnce(); 210 | loop_rate.sleep(); 211 | } 212 | } 213 | -------------------------------------------------------------------------------- /examples/laser_tag/src/robot_controller/youbot_interface.cpp: -------------------------------------------------------------------------------- 1 | #include "youbot_interface.h" 2 | 3 | #define TRANS_GOAL_TOLERANCE 0.05f // reach target with +-5cm tolerance 4 | #define TRANS_SPEED_FACTOR 0.8f 5 | 6 | #define ROT_GOAL_TOLERANCE 0.0872f // reach target with +5 deg tolerance 7 | #define ROT_SPEED_FACTOR 0.8f 8 | 9 | #define NORTH_THETA 1.57 // 90 deg from x axis 10 | 11 | #define CONTROL_RATE 10 // hz 12 | #define NUM_LASER_DIRECTIONS 8 13 | #define CONTROL_TIMEOUT 6 // wait for 6 seconds before giving up on the p-control 14 | 15 | #define RANDOM_SEED 45 16 | #define DEFAULT_NOISE_SIGMA 0.5 17 | 18 | #define OBSTACLE_BOX_COLLIDER_SIZE 0.90 // meters 19 | 20 | // From despot/src/util/util.cpp 21 | double erf(double x) { 22 | // constants 23 | double a1 = 0.254829592; 24 | double a2 = -0.284496736; 25 | double a3 = 1.421413741; 26 | double a4 = -1.453152027; 27 | double a5 = 1.061405429; 28 | double p = 0.3275911; 29 | // Save the sign of x 30 | int sign = 1; 31 | if (x < 0) 32 | sign = -1; 33 | x = fabs(x); 34 | // A&S formula 7.1.26 35 | double t = 1.0 / (1.0 + p * x); 36 | double y = 1.0 37 | - (((((a5 * t + a4) * t) + a3) * t + a2) * t + a1) * t * exp(-x * x); 38 | 39 | return sign * y; 40 | } 41 | 42 | // CDF of the normal distribution 43 | double gausscdf(double x, double mean, double sigma) { 44 | return 0.5 * (1 + erf((x - mean) / (sqrt(2) * sigma))); 45 | } 46 | 47 | 48 | YoubotInterface::YoubotInterface() 49 | { 50 | /* 51 | * Initialize controller and laser interface for the Youbot robot (without KUKA arm) 52 | */ 53 | 54 | nh_ = ros::NodeHandlePtr(new ros::NodeHandle()); 55 | 56 | laser_readings_.resize(NUM_LASER_DIRECTIONS); 57 | 58 | control_srv_ = nh_->advertiseService("youbot_discrete_controller", &YoubotInterface::DiscreteController, this); 59 | vel_pub_ = nh_->advertise("cmd_vel", 1); 60 | 61 | base_pose_sub_ = nh_->subscribe("odom", 30, &YoubotInterface::base_pose_cb, this); 62 | laser_sub_ = nh_->subscribe("laser_scan", 30, &YoubotInterface::laser_cb, this); 63 | 64 | if (nh_->getParam("youbot_interface/noise_sigma", noise_sigma_)) 65 | { 66 | ROS_INFO("Setting laser noise sigma to: %f", noise_sigma_); 67 | } 68 | else 69 | { 70 | noise_sigma_ = DEFAULT_NOISE_SIGMA; 71 | ROS_INFO("Setting laser to default noise sigma: %f", noise_sigma_); 72 | } 73 | 74 | srand(RANDOM_SEED); 75 | } 76 | 77 | void YoubotInterface::base_pose_cb(const nav_msgs::Odometry::ConstPtr& odom) 78 | { 79 | base_pose_ = odom->pose.pose; 80 | } 81 | 82 | 83 | void YoubotInterface::laser_cb(const sensor_msgs::LaserScan::ConstPtr& scan) 84 | { 85 | /* 86 | * Despot expects laser readings of empty cells (excluding the robot's cell) rounded to the nearest meter. 87 | * Raw laser readings go from -180deg to 180deg with 0deg = North 88 | */ 89 | 90 | std::vector raw_readings; 91 | raw_readings.resize(NUM_LASER_DIRECTIONS); 92 | 93 | raw_readings[SOUTH] = scan->ranges[0] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0); 94 | raw_readings[SOUTHEAST] = scan->ranges[1] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0) * sqrt(2.0); 95 | raw_readings[EAST] = scan->ranges[2] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0); 96 | raw_readings[NORTHEAST] = scan->ranges[3] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0) * sqrt(2.0); 97 | raw_readings[NORTH] = scan->ranges[4] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0); 98 | raw_readings[NORTHWEST] = scan->ranges[5] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0) * sqrt(2.0); 99 | raw_readings[WEST] = scan->ranges[6] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0); 100 | raw_readings[SOUTHWEST] = scan->ranges[7] + (OBSTACLE_BOX_COLLIDER_SIZE / 2.0) * sqrt(2.0); 101 | 102 | // sensor gaussian noise model 103 | double unit_size_ = 1; 104 | for (int d = 0; d < NUM_LASER_DIRECTIONS; d++) { 105 | double r = ((double) rand() / (RAND_MAX)); 106 | double accum_p = 0.0f; 107 | double dist = raw_readings[d]; 108 | int selected=-1; 109 | int reading = 0; 110 | for (reading = 0; reading < dist / unit_size_; reading++) { 111 | double min_noise = reading * unit_size_ - dist; 112 | double max_noise = std::min(dist, (reading + 1) * unit_size_) - dist; 113 | double prob = 114 | 2 115 | * (gausscdf(max_noise, 0, noise_sigma_) 116 | - (reading > 0 ? 117 | gausscdf(min_noise, 0, noise_sigma_) : 0 )); 118 | 119 | accum_p += prob; 120 | 121 | if (accum_p > r && selected==-1) 122 | selected=reading; 123 | } 124 | 125 | laser_readings_[d] = selected; 126 | } 127 | } 128 | 129 | geometry_msgs::Vector3 YoubotInterface::quat2euler(const geometry_msgs::Quaternion quat) 130 | { 131 | geometry_msgs::Vector3 euler; 132 | tf::Quaternion q(quat.x, quat.y, quat.z, quat.w); 133 | tf::Matrix3x3 m(q); 134 | m.getRPY(euler.x, euler.y, euler.z); 135 | return euler; 136 | } 137 | 138 | void YoubotInterface::Goto(float x, float y) 139 | { 140 | /* 141 | * Goto a given pose. No uncertainity involved in executing actions. 142 | * Simple P-controller. 143 | */ 144 | 145 | float curr_x = base_pose_.position.x; 146 | float curr_y = base_pose_.position.y; 147 | float curr_theta = this->quat2euler(base_pose_.orientation).z; 148 | 149 | float delta_x = x - curr_x; 150 | float delta_y = y - curr_y; 151 | float delta_theta = NORTH_THETA - curr_theta; 152 | geometry_msgs::Twist cmd = geometry_msgs::Twist(); 153 | ros::Rate loop_rate(CONTROL_RATE); 154 | 155 | // stop moving 156 | cmd.linear.x = 0.0f; 157 | cmd.linear.y = 0.0f; 158 | cmd.angular.z = 0.0f; 159 | vel_pub_.publish(cmd); 160 | 161 | ros::Time start_time = ros::Time::now(); 162 | while (ros::ok && (fabs(delta_x) > TRANS_GOAL_TOLERANCE || fabs(delta_y) > TRANS_GOAL_TOLERANCE || fabs(delta_theta) > ROT_GOAL_TOLERANCE) ) 163 | { 164 | curr_x = base_pose_.position.x; 165 | curr_y = base_pose_.position.y; 166 | curr_theta = this->quat2euler(base_pose_.orientation).z; 167 | 168 | delta_x = x - curr_x; 169 | delta_y = y - curr_y; 170 | delta_theta = NORTH_THETA - curr_theta; 171 | 172 | // transform vel to robot coordinate frame (90deg anti-clockwise rotation) 173 | // TODO: use TF transform to do this 174 | cmd.linear.x = delta_y * TRANS_SPEED_FACTOR; 175 | cmd.linear.y = -delta_x * TRANS_SPEED_FACTOR; 176 | cmd.angular.z = delta_theta * ROT_SPEED_FACTOR; 177 | vel_pub_.publish(cmd); 178 | 179 | ros::Time curr_time = ros::Time::now(); 180 | ros::Duration diff = curr_time - start_time; 181 | if (diff.toSec() > CONTROL_TIMEOUT) 182 | break; 183 | 184 | loop_rate.sleep(); 185 | ros::spinOnce(); 186 | } 187 | 188 | // stop moving 189 | cmd.linear.x = 0.0f; 190 | cmd.linear.y = 0.0f; 191 | cmd.angular.z = 0.0f; 192 | vel_pub_.publish(cmd); 193 | 194 | } 195 | 196 | bool YoubotInterface::DiscreteController(laser_tag::YoubotActionObs::Request &req, 197 | laser_tag::YoubotActionObs::Response &res) 198 | { 199 | std::string cmd = req.direction.c_str(); 200 | 201 | // always start at the center of a square 202 | float base_x = floor(base_pose_.position.x) + 0.5f; 203 | float base_y = floor(base_pose_.position.y) + 0.5f; 204 | 205 | float target_x = base_x; 206 | float target_y = base_y; 207 | 208 | ROS_INFO("Received Command: %s", cmd.c_str()); 209 | 210 | if (cmd == "North") 211 | { 212 | target_x = base_x; 213 | target_y = base_y + 1.0f; 214 | ROS_INFO("Going North..."); 215 | } 216 | else if (cmd == "East") 217 | { 218 | target_x = base_x + 1.0f; 219 | target_y = base_y; 220 | ROS_INFO("Going East..."); 221 | } 222 | else if (cmd == "South") 223 | { 224 | target_x = base_x; 225 | target_y = base_y - 1.0f; 226 | ROS_INFO("Going South..."); 227 | } 228 | else if (cmd == "West") 229 | { 230 | target_x = base_x - 1.0f; 231 | target_y = base_y; 232 | ROS_INFO("Going West..."); 233 | } 234 | else if (cmd == "NE") 235 | { 236 | target_x = base_x + 1.0f; 237 | target_y = base_y + 1.0f; 238 | ROS_INFO("Going NorthEast..."); 239 | } 240 | else if (cmd == "SE") 241 | { 242 | target_x = base_x + 1.0f; 243 | target_y = base_y - 1.0f; 244 | ROS_INFO("Going SouthEast..."); 245 | } 246 | else if (cmd == "SW") 247 | { 248 | target_x = base_x - 1.0f; 249 | target_y = base_y - 1.0f; 250 | ROS_INFO("Going SouthWest..."); 251 | } 252 | else if (cmd == "NW") 253 | { 254 | target_x = base_x - 1.0f; 255 | target_y = base_y + 1.0f; 256 | ROS_INFO("Going NorthWest"); 257 | } 258 | else if (cmd == "Stay") 259 | { 260 | target_x = base_x; 261 | target_y = base_y; 262 | ROS_INFO("Stay"); 263 | } 264 | else 265 | { 266 | ROS_ERROR("Invalid direction. Possible directions: North, East, South, West, NE, SE, SW, NW"); 267 | res.laser_readings = laser_readings_; 268 | return false; 269 | } 270 | 271 | this->Goto(target_x, target_y); 272 | 273 | res.laser_readings = laser_readings_; 274 | return true; 275 | } 276 | 277 | int main(int argc, char** argv) 278 | { 279 | ros::init(argc, argv, "youbot_interface"); 280 | YoubotInterface youbot_interface; 281 | 282 | // Main Loop 283 | ros::Rate loop_rate(CONTROL_RATE); 284 | while (true) 285 | { 286 | ros::spinOnce(); 287 | loop_rate.sleep(); 288 | } 289 | } 290 | -------------------------------------------------------------------------------- /examples/laser_tag/srv/TagActionObs.srv: -------------------------------------------------------------------------------- 1 | # 0 - NORTH 2 | # 1 - EAST 3 | # 2 - SOUTH 4 | # 3 - WEST 5 | # 4 - TAG 6 | 7 | int32 action 8 | --- 9 | int32[] observations 10 | bool tag_success 11 | -------------------------------------------------------------------------------- /examples/laser_tag/srv/YoubotActionObs.srv: -------------------------------------------------------------------------------- 1 | # directions 2 | # North - NORTH 3 | # East - EAST 4 | # South - SOUTH 5 | # West - WEST 6 | # NE - NORTHEAST 7 | # SE - SOUTHEAST 8 | # SW - SOUTHWEST 9 | # NW - NORTHWEST 10 | 11 | string direction 12 | --- 13 | int32[] laser_readings 14 | -------------------------------------------------------------------------------- /examples/laser_tag/test/test_laser_tag_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // for tests 5 | #include 6 | 7 | int main(int argc, char **argv) 8 | { 9 | // initilize ROS node. Should be done just once 10 | ros::init(argc, argv, "test_laser_tag"); 11 | ros::NodeHandle nh; 12 | 13 | // wait for controller service to show up (waits forever) 14 | ros::service::waitForService("laser_tag_action_obs", -1); 15 | 16 | // setup service client 17 | ros::ServiceClient client = nh.serviceClient("laser_tag_action_obs"); 18 | 19 | // send action & receive observation 20 | laser_tag::TagActionObs srv; 21 | srv.request.action = 0; // actions: 0 - North, 1 - East, 2 - South, 3 - West, 4 - Tag 22 | if (client.call(srv)) 23 | { 24 | // observations after executing actions 25 | std::vector laser_obs = srv.response.observations; 26 | 27 | // print observations (metric readings rounded to the nearest integer) 28 | ROS_INFO("Laser Observations"); 29 | ROS_INFO("North: %d" , laser_obs[0]); 30 | ROS_INFO("East: %d" , laser_obs[1]); 31 | ROS_INFO("South: %d" , laser_obs[2]); 32 | ROS_INFO("West: %d" , laser_obs[3]); 33 | ROS_INFO("NorthEast: %d", laser_obs[4]); 34 | ROS_INFO("SouthEast: %d", laser_obs[5]); 35 | ROS_INFO("SouthWest: %d", laser_obs[6]); 36 | ROS_INFO("NorthWest: %d", laser_obs[7]); 37 | } 38 | else 39 | { 40 | ROS_ERROR("Invalid Action OR Invalid Tag"); 41 | return 1; 42 | } 43 | 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /images/gazebo_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AdaCompNUS/despot_tutorials/cfcc27591d04f5a708fe04e4613d3012a96aa744/images/gazebo_screenshot.png -------------------------------------------------------------------------------- /robots/youbot/youbot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS 8 | ) 9 | 10 | install(DIRECTORY controller meshes robots urdf 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/README.md: -------------------------------------------------------------------------------- 1 | youbot_description 2 | ================== 3 | 4 | Robot description in form of URDF files and meshes 5 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/controller/ros_controller.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/meshes/sensors/asus_xtion_pro_live_base.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AdaCompNUS/despot_tutorials/cfcc27591d04f5a708fe04e4613d3012a96aa744/robots/youbot/youbot_description/meshes/sensors/asus_xtion_pro_live_base.png -------------------------------------------------------------------------------- /robots/youbot/youbot_description/meshes/sensors/asus_xtion_pro_live_camera.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AdaCompNUS/despot_tutorials/cfcc27591d04f5a708fe04e4613d3012a96aa744/robots/youbot/youbot_description/meshes/sensors/asus_xtion_pro_live_camera.png -------------------------------------------------------------------------------- /robots/youbot/youbot_description/meshes/sensors/hokuyo_convex.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | VCGLab 6 | VCGLib | MeshLab 7 | 8 | Y_UP 9 | Sat Jul 28 08:47:21 2012 10 | Sat Jul 28 08:47:21 2012 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -0.023 -0.025 -0.03505 0.02164 -0.02364 0.00545 -0.00528 0.01799 0.03505 -0.01856 0.00267 0.03505 -0.01417 0.01228 0.03505 -0.02331 0.02261 0.0057 -0.01706 0.00779 0.03505 0 0.01875 0.03505 0.00528 0.01799 0.03505 -0.02164 0.02364 0.00545 -0.02264 0.02337 0.00545 0.01706 -0.00779 0.03505 -0.02261 -0.02331 0.0057 -0.0225 -0.004 0.02545 -0.02264 -0.004 0.02495 -0.02364 -0.02164 0.00545 -0.02239 0.0045 0.02538 0.02264 0.02337 0.00545 -0.02261 0.02331 0.0057 -0.02164 0.02357 0.0057 0.01417 -0.01228 0.03505 0.02264 -0.02337 0.00545 0.01856 0.00267 0.03505 0 -0.01875 0.03505 -0.0225 0.004 0.02545 0.02164 0.02357 0.0057 0.01417 0.01228 0.03505 0.01706 0.00779 0.03505 0.02364 0.02164 0.00545 0.02331 0.02261 0.0057 0.02357 0.02164 0.0057 -0.024 0.02473 -0.03505 0.02261 -0.02331 0.0057 0.02164 -0.02357 0.0057 0.02364 -0.02164 0.00545 -0.00528 -0.01799 0.03505 -0.02164 -0.02364 0.00545 -0.02164 -0.02357 0.0057 -0.02264 0.004 0.02495 -0.025 0.023 -0.03505 -0.02357 0.02164 0.0057 -0.02364 0.02164 0.00545 -0.01706 -0.00779 0.03505 -0.01856 -0.00267 0.03505 -0.02331 -0.02261 0.0057 -0.02239 -0.0045 0.02538 -0.02357 -0.02164 0.0057 0.02164 0.02364 0.00545 0.02357 -0.02164 0.0057 0.02331 -0.02261 0.0057 -0.02264 -0.02337 0.00545 -0.02337 -0.02264 0.00545 -0.02337 0.02264 0.00545 0.02337 0.02264 0.00545 0.02473 0.024 -0.03505 0.024 0.02473 -0.03505 0.02261 0.02331 0.0057 0.01856 -0.00267 0.03505 -0.01417 -0.01228 0.03505 0.00528 -0.01799 0.03505 -0.01014 0.01577 0.03505 0.01014 0.01577 0.03505 0.01014 -0.01577 0.03505 -0.01014 -0.01577 0.03505 0.02473 -0.024 -0.03505 0.025 -0.023 -0.03505 -0.024 -0.02473 -0.03505 0.023 -0.025 -0.03505 0.023 0.025 -0.03505 0.025 0.023 -0.03505 -0.02473 -0.024 -0.03505 -0.025 -0.023 -0.03505 -0.02473 0.024 -0.03505 -0.023 0.025 -0.03505 0.024 -0.02473 -0.03505 0.02337 -0.02264 0.00545 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -0.260445 -0.964612 0.0411377 0.260445 -0.964612 0.0411377 0.260445 -0.964612 0.0411377 0 -0.986627 0.162993 -0.964612 -0.260445 0.0411377 -0.964612 -0.260445 0.0411377 -0.260445 -0.964612 0.0411377 -0.935632 -0.23656 0.261977 -0.591935 0.683524 0.427094 -0.384409 0.841544 0.379517 -0.880555 0.257975 0.397582 -0.766136 0.493125 0.412145 -0.640176 0.640176 0.424676 -0.866324 0.346709 0.359549 0.137626 0.956138 0.258572 0 0.986627 0.162993 -0.252094 0.933683 0.254333 -0.137626 0.956138 0.258572 0.260445 0.964612 0.0411378 0.137626 -0.956138 0.258572 0.906821 -0.26567 0.327253 0.766136 -0.493125 0.412145 -0.252094 -0.933683 0.254333 -0.935634 -0.236556 0.261974 -0.883119 0.249878 0.397066 0.384409 0.841544 0.379517 0.591935 0.683524 0.427094 0.252094 0.933683 0.254333 -0.24601 0.917808 0.311621 -0.249923 0.932405 0.261073 -0.153436 0.951561 0.266437 0.964612 0.260445 0.0411377 0.964612 0.260445 0.0411378 -0.260445 0.964612 0.0411378 0.384409 -0.841544 0.379517 0.591935 -0.683524 0.427093 0.252094 -0.933683 0.254333 0.640176 -0.640176 0.424675 0.950444 0.160728 0.266124 -0.137626 -0.956138 0.258572 -0.929305 0.249092 0.272663 -0.93261 0.242583 0.267193 -0.766136 -0.493125 0.412145 -0.640176 -0.640176 0.424675 -0.933683 -0.252094 0.254333 -0.932405 -0.249923 0.261073 0.153436 0.951561 0.266437 0.249923 0.932405 0.261073 0.24601 0.917808 0.311621 0.640176 0.640176 0.424676 0.766136 0.493125 0.412145 0.906821 0.26567 0.327253 0.916644 0.245698 0.315273 0.933683 0.252094 0.254333 0.932405 0.249923 0.261073 -0.260445 0.964612 0.0411378 0.153436 -0.951561 0.266437 0.249923 -0.932405 0.261073 0.24601 -0.917808 0.311621 0.964612 -0.260445 0.0411377 -0.591935 -0.683524 0.427093 -0.384409 -0.841544 0.379517 -0.249923 -0.932405 0.261073 -0.153436 -0.951561 0.266437 -0.24601 -0.917808 0.311621 -0.935634 0.236556 0.261974 -0.935632 0.23656 0.261977 -0.964612 0.260445 0.0411377 -0.964612 0.260445 0.0411378 -0.933683 0.252094 0.254333 -0.932405 0.249923 0.261073 -0.866324 -0.346709 0.359549 -0.880555 -0.257975 0.397582 -0.883119 -0.249878 0.397066 -0.93261 -0.242583 0.267193 -0.929305 -0.249092 0.272663 0.260445 0.964612 0.0411378 0.950444 -0.160728 0.266124 0.916644 -0.245698 0.315273 0.932405 -0.249923 0.261073 0.964612 -0.260445 0.0411377 0.933683 -0.252094 0.254333 0 -0.999437 0.0335613 0 -0.999437 0.0335613 -0.706311 -0.706311 0.0474362 -0.706311 -0.706311 0.0474362 -0.962965 0 0.269626 -0.962965 0 0.269626 -0.685253 0.685253 0.246689 -0.685254 0.685254 0.246689 0.706311 0.706311 0.0474362 0.706311 0.706311 0.0474362 -0.925116 0 0.379683 -0.925116 0 0.379683 -0.685254 -0.685254 0.246689 -0.685253 -0.685253 0.246689 0.685254 0.685254 0.246689 0.685253 0.685253 0.246689 -0.706311 0.706311 0.0474362 -0.706311 0.706311 0.0474362 0.985591 0 0.169149 0.985591 -7.27966e-09 0.169149 0.999437 0 0.0335614 0.999437 -1.44438e-09 0.0335614 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.998688 0 0.0512148 -0.998688 -2.20413e-09 0.0512148 -0.999437 0 0.0335614 -0.999437 -1.44438e-09 0.0335614 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0.999437 0.0335614 0 0.999437 0.0335614 0.706311 -0.706311 0.0474362 0.706311 -0.706311 0.0474362 0.685254 -0.685254 0.246689 0.685253 -0.685253 0.246689 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 |

36 0 66 0 0 0 74 1 1 1 67 1 74 2 21 2 1 2 36 3 1 3 23 3 71 4 70 4 15 4 51 5 15 5 70 5 36 6 50 6 66 6 46 7 13 7 15 7 4 8 60 8 18 8 60 9 2 9 18 9 6 10 16 10 3 10 4 11 5 11 6 11 4 12 18 12 5 12 5 13 16 13 6 13 47 14 7 14 8 14 47 15 9 15 7 15 10 16 18 16 9 16 7 17 9 17 2 17 55 18 47 18 17 18 1 19 59 19 23 19 11 20 49 20 57 20 20 21 49 21 11 21 12 22 50 22 36 22 13 23 14 23 15 23 16 24 24 24 3 24 61 25 56 25 8 25 26 26 56 26 61 26 47 27 56 27 17 27 2 28 19 28 18 28 19 29 9 29 18 29 9 30 19 30 2 30 28 31 69 31 54 31 28 32 54 32 53 32 9 33 31 33 10 33 59 34 32 34 62 34 62 35 32 35 20 35 32 36 1 36 21 36 32 37 49 37 20 37 28 38 30 38 22 38 23 39 35 39 36 39 5 40 40 40 16 40 40 41 24 41 16 41 42 42 44 42 58 42 44 43 12 43 58 43 51 44 44 44 15 44 15 45 44 45 46 45 8 46 25 46 47 46 47 47 25 47 56 47 56 48 25 48 8 48 29 49 56 49 26 49 27 50 29 50 26 50 22 51 29 51 27 51 30 52 29 52 22 52 53 53 29 53 28 53 28 54 29 54 30 54 31 55 9 55 73 55 1 56 33 56 59 56 32 57 33 57 1 57 32 58 59 58 33 58 65 59 34 59 64 59 12 60 63 60 58 60 35 61 63 61 12 61 36 62 37 62 12 62 35 63 37 63 36 63 35 64 12 64 37 64 24 65 41 65 38 65 40 66 41 66 24 66 39 67 41 67 72 67 72 68 41 68 52 68 52 69 41 69 5 69 40 70 5 70 41 70 45 71 44 71 42 71 43 72 45 72 42 72 43 73 13 73 45 73 46 74 45 74 13 74 44 75 45 75 46 75 55 76 68 76 47 76 48 77 34 77 57 77 49 78 48 78 57 78 34 79 48 79 49 79 34 80 75 80 64 80 34 81 49 81 75 81 67 82 1 82 0 82 0 83 1 83 36 83 50 84 70 84 66 84 70 85 50 85 51 85 13 86 24 86 14 86 14 87 24 87 38 87 5 88 18 88 52 88 10 89 52 89 18 89 53 90 54 90 17 90 54 91 55 91 17 91 24 92 13 92 43 92 24 93 43 93 3 93 50 94 44 94 51 94 12 95 44 95 50 95 29 96 53 96 17 96 56 97 29 97 17 97 10 98 72 98 52 98 31 99 72 99 10 99 57 100 34 100 22 100 22 101 34 101 28 101 69 102 34 102 65 102 28 103 34 103 69 103 6 104 63 104 4 104 42 105 58 105 63 105 8 106 63 106 61 106 57 107 63 107 11 107 59 108 62 108 63 108 22 109 63 109 57 109 2 110 63 110 7 110 6 111 3 111 63 111 60 112 63 112 2 112 26 113 61 113 63 113 20 114 63 114 62 114 60 115 4 115 63 115 8 116 7 116 63 116 26 117 63 117 27 117 59 118 63 118 23 118 20 119 11 119 63 119 22 120 27 120 63 120 43 121 42 121 63 121 63 122 35 122 23 122 43 123 63 123 3 123 14 124 38 124 41 124 14 125 41 125 15 125 71 126 41 126 39 126 15 127 41 127 71 127 64 128 68 128 65 128 70 129 68 129 66 129 67 130 68 130 74 130 71 131 39 131 68 131 65 132 68 132 69 132 67 133 0 133 68 133 66 134 68 134 0 134 64 135 74 135 68 135 70 136 71 136 68 136 54 137 68 137 55 137 54 138 69 138 68 138 72 139 31 139 68 139 72 140 68 140 39 140 73 141 68 141 31 141 9 142 47 142 68 142 73 143 9 143 68 143 64 144 75 144 74 144 74 145 75 145 21 145 21 146 75 146 32 146 32 147 75 147 49 147

45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/meshes/sensors/kinect.tga: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AdaCompNUS/despot_tutorials/cfcc27591d04f5a708fe04e4613d3012a96aa744/robots/youbot/youbot_description/meshes/sensors/kinect.tga -------------------------------------------------------------------------------- /robots/youbot/youbot_description/meshes/youbot_gripper/finger_convex.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | VCGLab 6 | VCGLib | MeshLab 7 | 8 | Y_UP 9 | Sat Jul 28 08:48:24 2012 10 | Sat Jul 28 08:48:24 2012 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 0.0070003 -0.0109028 -0.0120831 0.00700036 0.00685519 -0.00940305 0.0070003 0.00809717 -0.0120831 0.00700048 0.00397718 -0.00310081 -0.00700012 -0.0109028 -0.0120828 -0.00700024 -0.0109028 -0.00208301 0.00303629 -0.0109028 -0.0120831 -0.00700048 -0.00525677 -0.00822937 -0.00600026 -0.000902822 0.032917 -0.00700053 -0.00796872 -0.00666356 -0.0070003 -0.0100208 0.00291681 0.00700006 -0.0100208 0.00291646 0.00600018 -0.000902822 0.0329167 0.00700017 0.00709718 0.0329167 0.00249943 -0.0109028 -0.00208288 -0.00600026 0.00809717 0.032917 -0.00494239 0.00809717 0.00733125 -0.00700053 0.00516319 -0.00666356 -0.0070003 0.00809717 0.00291681 -0.00250026 0.00809717 -0.0100828 -0.00700053 -0.00483674 -0.00666362 -0.00700048 0.00474316 -0.00822937 -0.00700012 0.00809717 -0.0120828 0.00700006 0.00809717 0.00291646 0.00600018 0.00809717 0.0329167 0.00494221 0.00809717 0.00733203 -0.00700024 0.00709718 0.032917 -0.00700024 9.71656e-05 0.032917 0.00700017 9.71656e-05 0.0329167 0.00700053 -0.00602275 -0.00310081 0.00700053 -0.00678277 -0.00310081 0.00700018 -0.0109028 -0.00208336 -0.00250039 -0.0109028 -0.00208294 0.00303629 0.00809717 -0.0120831 0.00249955 0.00809717 -0.0100827 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 1 -8.36101e-05 1.20144e-05 0 0 -1 1 0 -2.63358e-05 1 0 -2.63358e-05 1 5.86737e-06 7.97299e-06 1 5.60477e-05 4.08087e-06 1 5.04273e-05 4.11301e-06 1 4.00816e-06 -2.00351e-05 1 5.86737e-06 -1.6236e-05 -2.96942e-05 0 -1 7.36044e-06 -0.956785 0.290796 -1 -9.44416e-05 4.36563e-06 -1 -0.000120304 -1.22006e-05 0 -1 0 0 0 -1 -2.96942e-05 0 -1 0 -1 0 0 -1 0 -1 -1.7273e-05 -6.79828e-05 -0.687818 -0.687817 0.231976 -1 -4.12777e-05 1.5753e-05 2.98012e-05 8.94066e-05 1 -0.706911 0.70691 0.0235645 8.66608e-06 -0.956785 0.290796 1 0.000596071 1.60813e-05 0.70691 0.706911 0.0235606 1.83689e-05 -0.984795 0.173718 4.39736e-06 -0.984793 0.173731 -1 -7.24461e-10 -3.80675e-05 0 1 0 -1.43442e-07 1 6.68693e-08 0.687811 -0.687825 0.231975 2.98012e-05 -8.94066e-05 1 1 -8.02927e-05 2.54475e-05 1 -0.000174079 5.49229e-05 -2.33701e-06 -0.984795 0.173718 -2.10332e-06 -0.984795 0.173719 -1 6.70322e-05 4.06596e-06 -1 0 0 -1 0 7.34129e-06 -1 -1.0413e-12 7.34129e-06 -1 0.000120249 -1.22317e-05 0 1 0 -1 0 -3.80677e-05 -1 2.33985e-10 -3.80663e-05 -1 4.85002e-05 -5.10765e-05 -1 0 -9.32909e-05 -1 0 -9.32909e-05 0 1 0 0 1 0 1.43484e-07 1 6.68699e-08 0 1 0 0 1 0 2.16962e-12 1 7.28032e-08 -5.71712e-12 1 7.27998e-08 0 0 1 0 0 1 1 0 1.01103e-05 1 0 1.01103e-05 2.29241e-05 0 1 2.29241e-05 0 1 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 |

31 0 0 0 30 0 0 1 33 1 2 1 29 2 30 2 0 2 29 3 0 3 2 3 29 4 3 4 13 4 2 5 13 5 1 5 13 6 3 6 1 6 1 7 29 7 2 7 29 8 1 8 3 8 33 9 4 9 22 9 8 10 10 10 11 10 5 11 10 11 9 11 4 12 5 12 9 12 6 13 0 13 31 13 6 14 33 14 0 14 6 15 4 15 33 15 6 16 31 16 5 16 6 17 5 17 4 17 7 18 4 18 9 18 27 19 10 19 8 19 9 20 10 20 27 20 13 21 24 21 15 21 15 22 18 22 26 22 8 23 11 23 12 23 23 24 13 24 2 24 23 25 24 25 13 25 11 26 14 26 31 26 14 27 11 27 10 27 20 28 7 28 9 28 18 29 16 29 22 29 15 30 16 30 18 30 28 31 12 31 11 31 8 32 12 32 28 32 28 33 31 33 30 33 28 34 11 34 31 34 10 35 5 35 32 35 32 36 14 36 10 36 17 37 26 37 18 37 17 38 20 38 9 38 17 39 27 39 26 39 27 40 17 40 9 40 17 41 18 41 22 41 16 42 19 42 22 42 21 43 7 43 20 43 21 44 20 44 17 44 21 45 17 45 22 45 4 46 21 46 22 46 21 47 4 47 7 47 2 48 34 48 25 48 25 49 23 49 2 49 23 50 25 50 24 50 25 51 34 51 19 51 25 52 19 52 16 52 25 53 15 53 24 53 25 54 16 54 15 54 26 55 27 55 15 55 15 56 27 56 8 56 28 57 29 57 13 57 30 58 29 58 28 58 28 59 15 59 8 59 13 60 15 60 28 60 14 61 32 61 31 61 32 62 5 62 31 62 34 63 33 63 22 63 34 64 2 64 33 64 19 65 34 65 22 65

45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_description 4 | 0.8.0 5 | Robot descriptions in form of URDF files and meshes 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | 13 | 14 | Frederik Heger --> 15 | 16 | catkin 17 | 18 | 19 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/robots/youbot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/robots/youbot_arm_only.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/robots/youbot_base_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/robots/youbot_base_only.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/robots/youbot_dual_arm.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/robots/youbot_target.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/common.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/materials.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/misc/cam3d_tower.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/misc/sensor_angle.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/.svn/all-wcprops: -------------------------------------------------------------------------------- 1 | K 25 2 | svn:wc:ra_dav:version-url 3 | V 91 4 | /svn/youbot/!svn/ver/206/youbot-ros-pkg/trunk/youbot_common/youbot_description/urdf/sensors 5 | END 6 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/.svn/entries: -------------------------------------------------------------------------------- 1 | 10 2 | 3 | dir 4 | 260 5 | http://youbot-store.com:444/svn/youbot/youbot-ros-pkg/trunk/youbot_common/youbot_description/urdf/sensors 6 | http://youbot-store.com:444/svn/youbot 7 | 8 | 9 | 10 | 2011-02-07T12:43:32.378745Z 11 | 206 12 | zakharov 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 2dec5107-7431-9d44-8047-c5e16bf51c85 28 | 29 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/asus_xtion_camera.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | R8G8B8 16 | 640 480 17 | 57 18 | 0.01 19 | 5 20 | 20.0 21 | 0.2 22 | 23 | true 24 | 20.0 25 | /camera/ir/image_raw 26 | /camera/ir/camera_info 27 | /camera/depth/image_raw 28 | /camera/depth/camera_info 29 | /camera/depth/points 30 | /${name}_depth_optical_frame 31 | 0.5 32 | 0.00000001 33 | 0.00000001 34 | 0.00000001 35 | 0.00000001 36 | 0.00000001 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | R8G8B8 46 | 640 480 47 | 57 48 | 0.01 49 | 5 50 | 20.0 51 | 0.2 52 | 53 | true 54 | 20.0 55 | /camera/rgb/image_raw 56 | /camera/rgb/camera_info 57 | /${name}_rgb_optical_frame 58 | 0.5 59 | 0.00000001 60 | 0.00000001 61 | 0.00000001 62 | 0.00000001 63 | 0.00000001 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/asus_xtion_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/hokuyo_urg04_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | true 12 | ${update_rate} 13 | 0 0 0 0 0 0 14 | false 15 | 16 | 17 | 18 | 9 19 | 1 20 | ${min_angle} 21 | ${max_angle} 22 | 23 | 24 | 25 | 0.1 26 | 30.0 27 | 0.01 28 | 29 | 30 | gaussian 31 | 34 | 0.0 35 | ${noise_stddev} 36 | 37 | 38 | 39 | 40 | 0.000 41 | true 42 | ${update_rate} 43 | ${ros_topic} 44 | /${name}_link 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/hokuyo_urg04_laser.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/hokuyo_urg04_laser_(perfect).gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | true 12 | ${update_rate} 13 | 0 0 0 0 0 0 14 | false 15 | 16 | 17 | 18 | 9 19 | 1 20 | ${min_angle} 21 | ${max_angle} 22 | 23 | 24 | 25 | 0.1 26 | 30.0 27 | 0.01 28 | 29 | 30 | gaussian 31 | 34 | 0.0 35 | ${noise_stddev} 36 | 37 | 38 | 39 | 40 | 0.000 41 | true 42 | ${update_rate} 43 | ${ros_topic} 44 | /${name}_link 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/hokuyo_urg04_laser_(perfect).urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/kinect_camera.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | R8G8B8 11 | 640 480 12 | 57 13 | 0.01 14 | 5 15 | 20.0 16 | 0.2 17 | 18 | true 19 | 20.0 20 | /camera/ir/image_raw 21 | /camera/ir/camera_info 22 | /camera/depth/image_raw 23 | /camera/depth/camera_info 24 | /camera/depth/points 25 | /${name}_depth_optical_frame 26 | 0.5 27 | 0.00000001 28 | 0.00000001 29 | 0.00000001 30 | 0.00000001 31 | 0.00000001 32 | 33 | 34 | 35 | 36 | R8G8B8 37 | 640 480 38 | 57 39 | 0.01 40 | 5 41 | 20.0 42 | 0.2 43 | 44 | true 45 | 20.0 46 | /camera/rgb/image_raw 47 | /camera/rgb/camera_info 48 | /${name}_rgb_optical_frame 49 | 0.5 50 | 0.00000001 51 | 0.00000001 52 | 0.00000001 53 | 0.00000001 54 | 0.00000001 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/kinect_camera.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/microsoft_lifecam.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | R8G8B8 11 | 1920 1080 12 | 70 13 | 0.01 14 | 5 15 | 20.0 16 | 0.2 17 | 18 | true 19 | 20.0 20 | /usb_cam/image_raw 21 | /usb_cam/camera_info 22 | /${name}_frame 23 | 0.00000001 24 | 0.00000001 25 | 0.00000001 26 | 0.00000001 27 | 0.00000001 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/sensors/microsoft_lifecam.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 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 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_arm/arm.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | false 13 | 14 | 15 | 16 | 17 | 18 | false 19 | 20 | 21 | 22 | 23 | 24 | false 25 | 26 | 27 | 28 | 29 | 30 | false 31 | 32 | 33 | 34 | 35 | 36 | false 37 | 38 | 39 | 40 | 41 | 42 | false 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_arm/arm.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | transmission_interface/SimpleTransmission 13 | 14 | EffortJointInterface 15 | 1 16 | 17 | 18 | EffortJointInterface 19 | 20 | 21 | 22 | 23 | transmission_interface/SimpleTransmission 24 | 25 | EffortJointInterface 26 | 1 27 | 28 | 29 | EffortJointInterface 30 | 31 | 32 | 33 | 34 | transmission_interface/SimpleTransmission 35 | 36 | EffortJointInterface 37 | 1 38 | 39 | 40 | EffortJointInterface 41 | 42 | 43 | 44 | 45 | transmission_interface/SimpleTransmission 46 | 47 | EffortJointInterface 48 | 1 49 | 50 | 51 | EffortJointInterface 52 | 53 | 54 | 55 | 56 | transmission_interface/SimpleTransmission 57 | 58 | EffortJointInterface 59 | 1 60 | 61 | 62 | EffortJointInterface 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_arm/arm.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_arm/limits.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | > 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 42 | 43 | 44 | 45 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | 56 | 57 | 59 | 60 | 61 | 62 | 63 | 65 | 66 | 67 | 68 | 69 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_base/base.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 1000000 17 | 100 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 1000000 28 | 100 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 1000000 39 | 100 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 1000000 50 | 100 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | cmd_vel 61 | odom 62 | odom 63 | 100.0 64 | base_footprint 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_base/base.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | transmission_interface/SimpleTransmission 15 | 16 | VelocityJointInterface 17 | ${wheel_mechanical_reduction} 18 | 19 | 20 | VelocityJointInterface 21 | 22 | 23 | 24 | 25 | transmission_interface/SimpleTransmission 26 | 27 | VelocityJointInterface 28 | ${caster_mechanical_reduction} 29 | 30 | 31 | VelocityJointInterface 32 | 33 | 34 | 35 | 36 | 37 | transmission_interface/SimpleTransmission 38 | 39 | VelocityJointInterface 40 | ${wheel_mechanical_reduction} 41 | 42 | 43 | VelocityJointInterface 44 | 45 | 46 | 47 | 48 | transmission_interface/SimpleTransmission 49 | 50 | VelocityJointInterface 51 | ${caster_mechanical_reduction} 52 | 53 | 54 | VelocityJointInterface 55 | 56 | 57 | 58 | 59 | 60 | transmission_interface/SimpleTransmission 61 | 62 | VelocityJointInterface 63 | ${wheel_mechanical_reduction} 64 | 65 | 66 | VelocityJointInterface 67 | 68 | 69 | 70 | 71 | transmission_interface/SimpleTransmission 72 | 73 | VelocityJointInterface 74 | ${caster_mechanical_reduction} 75 | 76 | 77 | VelocityJointInterface 78 | 79 | 80 | 81 | 82 | 83 | transmission_interface/SimpleTransmission 84 | 85 | VelocityJointInterface 86 | ${wheel_mechanical_reduction} 87 | 88 | 89 | VelocityJointInterface 90 | 91 | 92 | 93 | 94 | transmission_interface/SimpleTransmission 95 | 96 | VelocityJointInterface 97 | ${caster_mechanical_reduction} 98 | 99 | 100 | VelocityJointInterface 101 | 102 | 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_base/base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_gripper/gripper.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | Gazebo/Black 11 | false 12 | true 13 | 14 | 15 | 16 | Gazebo/Black 17 | false 18 | true 19 | 20 | 21 | 22 | Gazebo/Black 23 | false 24 | true 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_gripper/gripper.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | transmission_interface/SimpleTransmission 12 | 13 | EffortJointInterface 14 | 1 15 | 16 | 17 | EffortJointInterface 18 | 19 | 20 | 21 | 22 | transmission_interface/SimpleTransmission 23 | 24 | EffortJointInterface 25 | 1 26 | 27 | 28 | EffortJointInterface 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_gripper/gripper.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_gripper/limits.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /robots/youbot/youbot_description/urdf/youbot_plate/plate.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/README.md: -------------------------------------------------------------------------------- 1 | youbot_simulation 2 | ================= 3 | 4 | Packages to run the KUKA youBot in the Gazebo simulation with ROS 5 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_gazebo_control) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS 8 | ) 9 | 10 | install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) 11 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 12 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/config/arm_1_controller.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | type: effort_controllers/JointTrajectoryController 3 | joints: 4 | - arm_joint_1 5 | - arm_joint_2 6 | - arm_joint_3 7 | - arm_joint_4 8 | - arm_joint_5 9 | gains: 10 | arm_joint_1: {p: 2000.0, i: 0.01, d: 0.1} 11 | arm_joint_2: {p: 2000.0, i: 0.01, d: 0.1} 12 | arm_joint_3: {p: 2000.0, i: 0.01, d: 0.1} 13 | arm_joint_4: {p: 2000.0, i: 0.01, d: 0.1} 14 | arm_joint_5: {p: 2000.0, i: 0.01, d: 0.1} 15 | constraints: 16 | goal_time: 0.6 17 | arm_joint_1: 18 | goal: 0.1 19 | arm_joint_2: 20 | goal: 0.1 21 | arm_joint_3: 22 | goal: 0.1 23 | arm_joint_4: 24 | goal: 0.1 25 | arm_joint_5: 26 | goal: 0.1 27 | 28 | gripper_controller: 29 | type: effort_controllers/JointTrajectoryController 30 | joints: 31 | - gripper_finger_joint_l 32 | - gripper_finger_joint_r 33 | gains: 34 | gripper_finger_joint_l: {p: 40.0, d: 1.0} 35 | gripper_finger_joint_r: {p: 40.0, d: 1.0} 36 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/config/arm_2_controller.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | type: effort_controllers/JointTrajectoryController 3 | joints: 4 | - arm_2_joint_1 5 | - arm_2_joint_2 6 | - arm_2_joint_3 7 | - arm_2_joint_4 8 | - arm_2_joint_5 9 | gains: 10 | arm_2_joint_1: {p: 2000.0, i: 0.01, d: 0.1} 11 | arm_2_joint_2: {p: 2000.0, i: 0.01, d: 0.1} 12 | arm_2_joint_3: {p: 2000.0, i: 0.01, d: 0.1} 13 | arm_2_joint_4: {p: 2000.0, i: 0.01, d: 0.1} 14 | arm_2_joint_5: {p: 2000.0, i: 0.01, d: 0.1} 15 | constraints: 16 | goal_time: 0.6 17 | arm_2_joint_1: 18 | goal: 0.1 19 | arm_2_joint_2: 20 | goal: 0.1 21 | arm_2_joint_3: 22 | goal: 0.1 23 | arm_2_joint_4: 24 | goal: 0.1 25 | arm_2_joint_5: 26 | goal: 0.1 27 | 28 | gripper_controller: 29 | type: effort_controllers/JointTrajectoryController 30 | joints: 31 | - gripper_2_finger_joint_l 32 | - gripper_2_finger_joint_r 33 | gains: 34 | gripper_2_finger_joint_l: {p: 40.0, d: 1.0} 35 | gripper_2_finger_joint_r: {p: 40.0, d: 1.0} 36 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/config/joint_state_controller.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 100 4 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/launch/arm_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/launch/joint_state_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_gazebo_control 4 | 0.8.0 5 | Controller 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | controller_manager 19 | 20 | 21 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_robot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_gazebo_robot) 3 | find_package(catkin REQUIRED) 4 | 5 | catkin_package( 6 | CATKIN_DEPENDS 7 | ) 8 | 9 | install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 10 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_robot/launch/youbot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_robot/launch/youbot_arm_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_robot/launch/youbot_base_only.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_robot/launch/youbot_dual_arm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_robot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_gazebo_robot 4 | 0.8.0 5 | Launch the KUKA youBot in the Gazebo simulation 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | xacro 19 | youbot_gazebo_control 20 | youbot_gazebo_worlds 21 | youbot_description 22 | gazebo_ros 23 | robot_state_publisher 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_gazebo_worlds) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | CATKIN_DEPENDS 8 | ) 9 | 10 | install(DIRECTORY launch urdf 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/launch/empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/launch/robocup_at_work_2012.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/launch/tower_of_hanoi.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_gazebo_worlds 4 | 0.8.0 5 | Gazebo worlds configurations 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | gazebo_ros 19 | xacro 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/urdf/robocup_at_work_2012.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | ---------- 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | Gazebo/Gray 262 | true 263 | 264 | 265 | Gazebo/Gray 266 | true 267 | 268 | 269 | Gazebo/Gray 270 | true 271 | 272 | 273 | Gazebo/Gray 274 | true 275 | 276 | 277 | Gazebo/Gray 278 | true 279 | 280 | 281 | Gazebo/Gray 282 | true 283 | 284 | 285 | Gazebo/Gray 286 | true 287 | 288 | 289 | Gazebo/Gray 290 | true 291 | 292 | 293 | Gazebo/Gray 294 | true 295 | 296 | 297 | Gazebo/Gray 298 | true 299 | 300 | 301 | 302 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_gazebo_worlds/urdf/tower_of_hanoi.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | Gazebo/Red 148 | true 149 | 150 | 151 | Gazebo/Blue 152 | true 153 | 154 | 155 | Gazebo/Blue 156 | true 157 | 158 | 159 | Gazebo/Blue 160 | true 161 | 162 | 163 | Gazebo/Red 164 | true 165 | 166 | 167 | Gazebo/Red 168 | true 169 | 170 | 171 | 172 | 173 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(youbot_simulation) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /robots/youbot/youbot_simulation/youbot_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | youbot_simulation 4 | 0.8.0 5 | Packages to run the KUKA youBot in the Gazebo simulation with ROS 6 | 7 | Walter Nowak 8 | Sebastian Blumenthal 9 | 10 | GPLv3 11 | 12 | http://github.com/mas-group/youbot_simulation --> 13 | 14 | Frederik Hegger 15 | 16 | catkin 17 | 18 | youbot_gazebo_control 19 | youbot_gazebo_robot 20 | youbot_gazebo_worlds 21 | 22 | 23 | 24 | 25 | 26 | --------------------------------------------------------------------------------