├── blue_simulator
    ├── CMakeLists.txt
    └── package.xml
├── .gitmodules
├── blue_gazebo
    ├── launch
    │   ├── full.launch
    │   ├── full_pickup.launch
    │   └── include
    │   │   ├── pickup_world_objects.launch
    │   │   ├── single_arm_gazebo.launch.xml
    │   │   └── run.launch.xml
    ├── package.xml
    ├── worlds
    │   └── empty_world.world
    ├── objects
    │   ├── simple_box.urdf
    │   ├── simple_cylinder.urdf
    │   └── table.urdf.xacro
    └── CMakeLists.txt
├── LICENSE
└── README.md


/blue_simulator/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(blue_simulator)
3 | find_package(catkin REQUIRED)
4 | catkin_metapackage()
5 | 


--------------------------------------------------------------------------------
/.gitmodules:
--------------------------------------------------------------------------------
1 | [submodule "roboticsgroup_gazebo_plugins"]
2 | 	path = roboticsgroup_gazebo_plugins
3 | 	url = https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins.git
4 | 


--------------------------------------------------------------------------------
/blue_gazebo/launch/full.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="version" default="2" doc="What version of the Blue arm hardware are we using? (1 or 2)" />
 3 |   <arg name="rosbridge_port" default="9090" doc="Port for rosbridge and the Python API" />
 4 | 
 5 |   <include
 6 |     file="$(find blue_gazebo)/launch/include/run.launch.xml">
 7 |     <arg name="configuration" value="full" />
 8 |     <arg name="version" value="$(arg version)" />
 9 |     <arg name="rosbridge_port" value="$(arg rosbridge_port)" />
10 |   </include>
11 | </launch>
12 | 


--------------------------------------------------------------------------------
/blue_simulator/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>blue_simulator</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The blue simulator metapackage contains tools for simulating the Blue robot</description>
 6 | 
 7 |   <maintainer email="phil80301@berkeley.edu"></maintainer>
 8 | 
 9 |   <license>MIT</license>
10 | 
11 |   <author email="phil80301@berkeley.edu">Philipp Wu</author>
12 |   <author email="brentyi@berkeley.edu">Brent Yi</author>
13 | 
14 |   <buildtool_depend>catkin</buildtool_depend>
15 | </package>
16 | 


--------------------------------------------------------------------------------
/blue_gazebo/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package format="2">
 3 |   <name>blue_gazebo</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The blue_gazebo package</description>
 6 | 
 7 |   <maintainer email="phil80301@berkeley.edu">Philipp Wu</maintainer>
 8 | 
 9 |   <license>MIT</license>
10 | 
11 |   <author email="phil80301@berkeley.edu">Philipp Wu</author>
12 |   <author email="brentyi@berkeley.edu">Brent Yi</author>
13 | 
14 |   <buildtool_depend>catkin</buildtool_depend>
15 | 
16 |   <depend>gazebo_ros_control</depend>
17 |   <exec_depend>blue_core</exec_depend>
18 | </package>
19 | 


--------------------------------------------------------------------------------
/blue_gazebo/worlds/empty_world.world:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0" ?>
 2 | <gazebo version="1.0">
 3 |   <world name="default">
 4 |     <!-- A global light source -->
 5 |     <physics type="ode">
 6 |       <gravity xyz="0 0 -9.81"/>
 7 |       <ode>
 8 |         <solver type="quick" dt="0.001" iters="10" sor="1.3"/>
 9 |         <constraints cfm="0.0" erp="0.2" contact_max_correcting_vel="100.0" contact_surface_layer="0.001"/>
10 |       </ode>
11 |     </physics>
12 |     <include>
13 |       <uri>model://sun</uri>
14 |     </include>
15 |     <!-- A ground plane -->
16 |     <include>
17 |       <uri>model://ground_plane</uri>
18 |     </include>
19 |   </world>
20 | </gazebo>
21 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 Berkeley Open Arms
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/blue_gazebo/objects/simple_box.urdf:
--------------------------------------------------------------------------------
 1 | <robot name="simple_box">
 2 |   <link name="my_box">
 3 |     <inertial>
 4 |       <mass value="0.5" />
 5 |       <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
 6 |       <origin xyz="0 0 0" /> 
 7 |       <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="1.0"  iyz="0.0"  izz="1.0" />
 8 |     </inertial>
 9 |     <visual>
10 |       <!-- visual origin is defined w.r.t. link local coordinate system -->
11 |       <origin xyz="0 0 0" rpy="0 0 0" />
12 |       <geometry>
13 |         <box size="0.07 0.07 0.20" />
14 |       </geometry>
15 |     </visual>
16 |     <collision>
17 |       <!-- collision origin is defined w.r.t. link local coordinate system -->
18 |       <origin xyz="0 0 0" rpy="0 0 0 " />
19 |       <geometry>
20 |         <box size="0.07 0.07 0.20" />
21 |       </geometry>
22 |       <friction>
23 |         <ode>
24 |           <mu>0.6</mu>
25 |           <mu2>0.6</mu2>
26 |         </ode>
27 |       </friction>
28 |     </collision>
29 |   </link>
30 |   <gazebo reference="my_box">
31 |     <material>Gazebo/Blue</material>
32 |     <turnGravityOff>false</turnGravityOff>
33 |   </gazebo>
34 | </robot>
35 | 


--------------------------------------------------------------------------------
/blue_gazebo/objects/simple_cylinder.urdf:
--------------------------------------------------------------------------------
 1 | <robot name="simple_cylinder">
 2 |   <link name="my_cylinder">
 3 |     <inertial>
 4 |       <mass value="0.25" />
 5 |       <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
 6 |       <origin xyz="0 0 0" /> 
 7 |       <inertia  ixx="1.0" ixy="0.0"  ixz="0.0"  iyy="1.0"  iyz="0.0"  izz="1.0" />
 8 |     </inertial>
 9 |     <visual>
10 |       <!-- visual origin is defined w.r.t. link local coordinate system -->
11 |       <origin xyz="0 0 0" rpy="0 0 0" />
12 |       <geometry>
13 |         <cylinder length="0.2" radius="0.025" />
14 |       </geometry>
15 |     </visual>
16 |     <collision>
17 |       <!-- collision origin is defined w.r.t. link local coordinate system -->
18 |       <origin xyz="0 0 0" rpy="0 0 0 " />
19 |       <geometry>
20 |         <cylinder length="0.2" radius="0.025" />
21 |       </geometry>
22 |       <friction>
23 |         <ode>
24 |           <mu>0.6</mu>
25 |           <mu2>0.6</mu2>
26 |         </ode>
27 |       </friction>
28 |     </collision>
29 |   </link>
30 |   <gazebo reference="my_cylinder">
31 |     <material>Gazebo/Blue</material>
32 |     <turnGravityOff>false</turnGravityOff>
33 |   </gazebo>
34 | </robot>
35 | 


--------------------------------------------------------------------------------
/blue_gazebo/launch/full_pickup.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="version" default="2" doc="What version of the Blue arm hardware are we using? (1 or 2)" />
 3 |   <arg name="rosbridge_port" default="9090" doc="Port for rosbridge and the Python API" />
 4 | 
 5 |   <!-- Spawn Blue -->
 6 |   <include
 7 |     file="$(find blue_gazebo)/launch/full.launch"
 8 |     pass_all_args="true"/>
 9 | 
10 |   <!-- Spawn objects in pickup world -->
11 |   <include
12 |     file="$(find blue_gazebo)/launch/include/pickup_world_objects.launch" />
13 | 
14 |   <node
15 |     name="$(anon blue_controller_spawner)"
16 |     pkg="blue_controller_manager"
17 |     type="spawner.py"
18 |     output="screen"
19 |     ns="right_arm">
20 |     <rosparam param="start_controllers">
21 |       - blue_controllers/joint_trajectory_controller
22 |       - blue_controllers/gripper_controller
23 |     </rosparam>
24 |   </node>
25 |   <node
26 |     name="$(anon blue_controller_spawner)"
27 |     pkg="blue_controller_manager"
28 |     type="spawner.py"
29 |     output="screen"
30 |     ns="left_arm">
31 |     <rosparam param="start_controllers">
32 |       - blue_controllers/joint_trajectory_controller
33 |       - blue_controllers/gripper_controller
34 |     </rosparam>
35 |   </node>
36 | 
37 | </launch>
38 | 


--------------------------------------------------------------------------------
/blue_gazebo/launch/include/pickup_world_objects.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <!-- Spawn a table -->
 3 |   <param
 4 |     name="table_description"
 5 |     command="$(find xacro)/xacro.py $(find blue_gazebo)/objects/table.urdf.xacro" />
 6 |   <node
 7 |     name="spawn_table"
 8 |     pkg="gazebo_ros"
 9 |     type="spawn_model"
10 |     args="-urdf -param table_description -z 0.01 -x -0.4 -model table_model"
11 |     respawn="false"
12 |     output="screen" />
13 | 
14 |   <!-- Spawn a box -->
15 |   <!-- <param -->
16 |     <!-- name="box_description" -->
17 |     <!-- command="$(find xacro)/xacro.py $(find blue_gazebo)/objects/simple_box.urdf" /> -->
18 |   <!-- <node -->
19 |     <!-- name="spawn_box" -->
20 |     <!-- pkg="gazebo_ros" -->
21 |     <!-- type="spawn_model" -->
22 |     <!-- args="-urdf -param box_description -x 0.8 -y 0.4 -z 1.5 -model box_model" -->
23 |     <!-- respawn="false" -->
24 |     <!-- output="screen" /> -->
25 | 
26 |   <!-- Spawn a cylinder -->
27 |   <param
28 |     name="box_description"
29 |     command="$(find xacro)/xacro.py $(find blue_gazebo)/objects/simple_cylinder.urdf" />
30 |   <node
31 |     name="spawn_cylinder"
32 |     pkg="gazebo_ros"
33 |     type="spawn_model"
34 |     args="-urdf -param box_description -x 0.8 -y 0.4 -z 1.5 -model cylinder_model"
35 |     respawn="false"
36 |     output="screen" />
37 | </launch>
38 | 


--------------------------------------------------------------------------------
/blue_gazebo/launch/include/single_arm_gazebo.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="side" doc="Side of the robot: left or right" />
 3 | 
 4 |   <rosparam command="load" file="$(find blue_controllers)/config/gazebo_mimic_joint_pid.yaml" />
 5 | 
 6 |   <group ns="$(arg side)_arm" clear_params="true">
 7 | 
 8 |     <!-- Use shared URDF -->
 9 |     <remap
10 |       from="robot_description"
11 |       to="/robot_description" />
12 | 
13 |     <!-- Load hardware configuration -->
14 |     <rosparam
15 |       file="$(find blue_bringup)/config/robot_parameters_$(arg side).yaml"
16 |       command="load" />
17 | 
18 |     <!-- Start the control stack -->
19 |     <!-- http://wiki.ros.org/ros_control -->
20 |     <rosparam
21 |       param="blue_controllers"
22 |       file="$(find blue_controllers)/config/gazebo_controllers.yaml"
23 |       subst_value="true" />
24 | 
25 |     <node
26 |       name="$(anon blue_controller_spawner)"
27 |       pkg="blue_controller_manager"
28 |       type="spawner.py"
29 |       output="screen">
30 |       <rosparam param="start_controllers">
31 |         - blue_controllers/joint_state_controller
32 |       </rosparam>
33 |     </node>
34 | 
35 |     <!-- Inverse kinematics service -->
36 |     <node
37 |       name="ik_service"
38 |       pkg="blue_bringup"
39 |       type="inverse_kinematics_service.py"
40 |       output="screen" />
41 | 
42 |   </group>
43 | 
44 | </launch>
45 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # blue_simulator
 2 | (Experimental) ROS package for simulating Blue via Gazebo.
 3 | 
 4 | ### Install instructions
 5 | 
 6 | - Install Ubuntu 16.0.4
 7 | - [Install ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu)
 8 |   - Start from 1.2 to 1.7, just copy paste into terminal
 9 |   - For step 1.4, use Desktop-Full if unsure
10 | - Create a workspace:
11 |   ```bash
12 |   mkdir -p ~/blue_ws/src && cd "$_"
13 |   ```
14 | - Clone the required packages:
15 |   ```bash
16 |   git clone https://github.com/berkeleyopenarms/blue_core.git
17 |   git clone https://github.com/berkeleyopenarms/blue_simulator.git
18 |   ```
19 | - Get the mimic joint plugin:
20 |   ```bash
21 |   cd blue_simulator
22 |   git submodule update --init
23 |   ```
24 | 
25 | - Install dependencies:
26 |   ```bash
27 |   cd ~/blue_ws
28 |   rosdep install --from-paths src --ignore-src -r -y
29 |   ```
30 | - Build:
31 |   ```bash
32 |   catkin_make install
33 |   ```
34 | - Source:
35 |   ```bash
36 |   echo "source ~/blue_ws/devel/setup.bash" >> ~/.bashrc
37 |   source ~/blue_ws/devel/setup.bash
38 |   ```
39 | 
40 | ### Instructions for use
41 | To launch the simulator (`full.launch` corresponds to the full, two-arm setup; `right.launch` or `left.launch` can also be used):
42 | ```bash
43 | roslaunch blue_gazebo full.launch
44 | ```
45 | 
46 | Running rviz afterwards is the same as with the physical robot:
47 | ```bash
48 | roslaunch blue_bringup rviz.launch
49 | ```
50 | 
51 | ### Notes
52 | * The gripper can be controlled using the `/(right/left)_arm/blue_controllers/gripper_controller` controller, but the URDF currently relies on [mimic joints](http://wiki.ros.org/urdf/XML/joint) and will cause Gazebo to crash if the gripper grasps an object. This is because the mimic joint plugin used to implement mimic joints in Gazebo will exert instantaneous forces which crashes the simulation.
53 | 


--------------------------------------------------------------------------------
/blue_gazebo/launch/include/run.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <!-- What kind of robot am I? -->
 4 |   <arg name="configuration" default="full" doc="left, right, or full" />
 5 |   <arg name="version" default="2" doc="1 or 2" />
 6 | 
 7 |   <arg name="rosbridge_port" default="9090" doc="port for rosbridge_server" />
 8 | 
 9 |   <!-- joint states topic -->
10 |   <remap
11 |     from="/right_arm/joint_states"
12 |     to="/joint_states" />
13 |   <remap
14 |     from="/left_arm/joint_states"
15 |     to="/joint_states" />
16 | 
17 |   <!-- Launch Gazebo-->
18 |   <include
19 |     file="$(find gazebo_ros)/launch/empty_world.launch">
20 |     <arg name="world_name" default="$(find blue_gazebo)/worlds/empty_world.world"/>
21 |   </include>
22 | 
23 |   <!-- Load URDF -->
24 |   <param
25 |     name="robot_description"
26 |     command="$(find xacro)/xacro --inorder $(find blue_descriptions)/robots/blue_full_v$(arg version).urdf.xacro is_gazebo:=true" />
27 | 
28 |   <!-- Spawn robot in Gazebo-->
29 |   <node
30 |     name="spawn_blue"
31 |     pkg="gazebo_ros"
32 |     type="spawn_model"
33 |     args="-param robot_description -urdf -model blue_full -z 0.965"
34 |     output="screen"/>
35 | 
36 | 
37 |   <!-- Run core control stack -->
38 |   <include
39 |     file="$(find blue_gazebo)/launch/include/single_arm_gazebo.launch.xml"
40 |     if="$(eval configuration == 'right' or configuration == 'full')">
41 |     <arg name="side" value="right"/>
42 |   </include>
43 |   <include
44 |     file="$(find blue_gazebo)/launch/include/single_arm_gazebo.launch.xml"
45 |     if="$(eval configuration == 'left' or configuration == 'full')">
46 |     <arg name="side" value="left"/>
47 |   </include>
48 | 
49 |   <!-- Compute forward kinematics and broadcast via tf -->
50 |   <node
51 |     name="robot_state_publisher"
52 |     pkg="robot_state_publisher"
53 |     type="robot_state_publisher"
54 |     respawn="false"
55 |     output="screen">
56 |   </node>
57 | 
58 |   <!-- Support external applications via rosbridge -->
59 |   <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" >
60 |     <arg name="port" value="$(arg rosbridge_port)" />
61 |   </include>
62 |   <node name="tf2_web_republisher"
63 |     pkg="tf2_web_republisher"
64 |     type="tf2_web_republisher" />
65 | 
66 | </launch>
67 | 


--------------------------------------------------------------------------------
/blue_gazebo/objects/table.urdf.xacro:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0"?>
  2 | <robot name="table"
  3 |        xmlns:xi="http://www.w3.org/2001/XInclude"
  4 |        xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
  5 |        xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
  6 |        xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
  7 |        xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
  8 |        xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
  9 |        xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
 10 |        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
 11 |        xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
 12 |        xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
 13 |        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
 14 |        xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
 15 | 
 16 |   <property name="table_height" value="0.8" />
 17 |   <property name="table_width" value="1.0" />
 18 |   <property name="table_depth" value="2.0" />
 19 |   <property name="leg_radius" value="0.04" />
 20 |   <property name="table_x" value="0.98" />
 21 |   <property name="table_y" value="0.0" />
 22 |   <property name="table_z" value="0.0" />
 23 | 
 24 |   <property name="table_top_thickness" value="0.075"/>
 25 | 
 26 |   <property name="M_PI" value="3.1415926535897931" />
 27 | 
 28 | 
 29 |   <!-- tabletop height is .55+.01+.025=.585 -->
 30 |   <link name="table_top_link">
 31 |     <inertial>
 32 |       <mass value="1.0" />
 33 |       <origin xyz="${table_x} ${table_y} ${table_z+table_height-table_top_thickness/2}" />
 34 |       <inertia ixx="1" ixy="0"  ixz="0"
 35 |                iyy="1" iyz="0"
 36 |                izz="1" />
 37 |     </inertial> 
 38 |     <visual>
 39 |       <origin xyz="${table_x} ${table_y} ${table_z+table_height-table_top_thickness/2}" />
 40 |       <geometry>
 41 |         <box size="${table_width} ${table_depth} ${table_top_thickness}" />
 42 |       </geometry>
 43 |     </visual> 
 44 |     <collision>
 45 |       <origin xyz="${table_x} ${table_y} ${table_z+table_height-table_top_thickness/2}" />
 46 |       <geometry>
 47 |         <box size="${table_width} ${table_depth} ${table_top_thickness}" />
 48 |       </geometry>
 49 |     </collision>
 50 |   </link>
 51 |   <gazebo reference="table_top_link">
 52 |     <material>Gazebo/Wood</material>
 53 |     <mu1>50.0</mu1>
 54 |     <mu2>50.0</mu2>
 55 |     <kp>1000000.0</kp>
 56 |     <kd>1.0</kd>
 57 |   </gazebo>
 58 | 
 59 |   <joint name="leg1_joint" type="fixed" >
 60 |     <parent link="table_top_link" />
 61 |     <origin xyz="${table_x+table_width/2} ${table_y+table_depth/2} ${table_z+table_height}" rpy="0 0 0" />
 62 |     <child link="leg1_link" />
 63 |   </joint> 
 64 |   <link name="leg1_link">
 65 |     <inertial>
 66 |       <mass value="1.0" />
 67 |       <origin xyz="0 0 ${-table_height/2}" />
 68 |       <inertia ixx="0.1" ixy="0"  ixz="0"
 69 |                iyy="0.1" iyz="0"
 70 |                izz="0.01" />
 71 |     </inertial> 
 72 |     <visual>
 73 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0 0 0" />
 74 |       <geometry>
 75 |         <cylinder radius="${leg_radius}" length="${table_height}" />
 76 |       </geometry>
 77 |     </visual> 
 78 |     <collision>
 79 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0.0 0.0 0.0" />
 80 |       <geometry>
 81 |         <cylinder radius="${leg_radius}" length="${table_height}" />
 82 |       </geometry>
 83 |     </collision>
 84 |   </link>
 85 |   <gazebo reference="leg1_link">
 86 |     <material>Gazebo/Red</material>
 87 |     <mu1>1000.0</mu1>
 88 |     <mu2>1000.0</mu2>
 89 |     <kp>10000000.0</kp>
 90 |     <kd>1.0</kd>
 91 |     <selfCollide>true</selfCollide>
 92 |   </gazebo>
 93 | 
 94 |   <joint name="leg2_joint" type="fixed" >
 95 |     <parent link="table_top_link" />
 96 |     <origin xyz="${table_x-table_width/2} ${table_y+table_depth/2} ${table_z+table_height}" rpy="0 0 0" />
 97 |     <child link="leg2_link" />
 98 |   </joint> 
 99 |   <link name="leg2_link">
100 |     <inertial>
101 |       <mass value="1.0" />
102 |       <origin xyz="0 0 ${-table_height/2}" />
103 |       <inertia ixx="0.1" ixy="0"  ixz="0"
104 |                iyy="0.1" iyz="0"
105 |                izz="0.01" />
106 |     </inertial> 
107 |     <visual>
108 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0 0 0" />
109 |       <geometry>
110 |         <cylinder radius="${leg_radius}" length="${table_height}" />
111 |       </geometry>
112 |     </visual> 
113 |     <collision>
114 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0.0 0.0 0.0" />
115 |       <geometry>
116 |         <cylinder radius="${leg_radius}" length="${table_height}" />
117 |       </geometry>
118 |     </collision>
119 |   </link>
120 |   <gazebo reference="leg2_link">
121 |     <material>Gazebo/Red</material>
122 |     <mu1>1000.0</mu1>
123 |     <mu2>1000.0</mu2>
124 |     <kp>10000000.0</kp>
125 |     <kd>1.0</kd>
126 |     <selfCollide>true</selfCollide>
127 |   </gazebo>
128 | 
129 |   <joint name="leg3_joint" type="fixed" >
130 |     <parent link="table_top_link" />
131 |     <origin xyz="${table_x+table_width/2} ${table_y-table_depth/2} ${table_z+table_height}" rpy="0 0 0" />
132 |     <child link="leg3_link" />
133 |   </joint> 
134 |   <link name="leg3_link">
135 |     <inertial>
136 |       <mass value="1.0" />
137 |       <origin xyz="0 0 ${-table_height/2}" />
138 |       <inertia ixx="0.1" ixy="0"  ixz="0"
139 |                iyy="0.1" iyz="0"
140 |                izz="0.01" />
141 |     </inertial> 
142 |     <visual>
143 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0 0 0" />
144 |       <geometry>
145 |         <cylinder radius="${leg_radius}" length="${table_height}" />
146 |       </geometry>
147 |     </visual> 
148 |     <collision>
149 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0.0 0.0 0.0" />
150 |       <geometry>
151 |         <cylinder radius="${leg_radius}" length="${table_height}" />
152 |       </geometry>
153 |     </collision>
154 |   </link>
155 |   <gazebo reference="leg3_link">
156 |     <material>Gazebo/Red</material>
157 |     <mu1>1000.0</mu1>
158 |     <mu2>1000.0</mu2>
159 |     <kp>10000000.0</kp>
160 |     <kd>1.0</kd>
161 |     <selfCollide>true</selfCollide>
162 |   </gazebo>
163 | 
164 |   <joint name="leg4_joint" type="fixed" >
165 |     <parent link="table_top_link" />
166 |     <origin xyz="${table_x-table_width/2} ${table_y-table_depth/2} ${table_z+table_height}" rpy="0 0 0" />
167 |     <child link="leg4_link" />
168 |   </joint> 
169 |   <link name="leg4_link">
170 |     <inertial>
171 |       <mass value="1.0" />
172 |       <origin xyz="0 0 ${-table_height/2}" />
173 |       <inertia ixx="0.1" ixy="0"  ixz="0"
174 |                iyy="0.1" iyz="0"
175 |                izz="0.01" />
176 |     </inertial> 
177 |     <visual>
178 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0 0 0" />
179 |       <geometry>
180 |         <cylinder radius="${leg_radius}" length="${table_height}" />
181 |       </geometry>
182 |     </visual> 
183 |     <collision>
184 |       <origin xyz="0.0 0.0 ${-table_height/2}" rpy="0.0 0.0 0.0" />
185 |       <geometry>
186 |         <cylinder radius="${leg_radius}" length="${table_height}" />
187 |       </geometry>
188 |     </collision>
189 |   </link>
190 |   <gazebo reference="leg4_link">
191 |     <material>Gazebo/Red</material>
192 |     <mu1>1000.0</mu1>
193 |     <mu2>1000.0</mu2>
194 |     <kp>10000000.0</kp>
195 |     <kd>1.0</kd>
196 |     <selfCollide>true</selfCollide>
197 |   </gazebo>
198 |   <gazebo>
199 |     <static>true</static>
200 |     <canonicalBody>table_top_link</canonicalBody>
201 |   </gazebo>
202 | 
203 | 
204 | </robot>
205 | 


--------------------------------------------------------------------------------
/blue_gazebo/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(blue_gazebo)
  3 | 
  4 | ## Compile as C++11, supported in ROS Kinetic and newer
  5 | # add_compile_options(-std=c++11)
  6 | 
  7 | ## Find catkin macros and libraries
  8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  9 | ## is used, also find other catkin packages
 10 | find_package(catkin REQUIRED COMPONENTS
 11 |   geometry_msgs
 12 |   interactive_markers
 13 |   kdl_parser
 14 |   roscpp
 15 |   std_msgs
 16 |   tf
 17 |   visualization_msgs
 18 | )
 19 | 
 20 | ## System dependencies are found with CMake's conventions
 21 | # find_package(Boost REQUIRED COMPONENTS system)
 22 | 
 23 | 
 24 | ## Uncomment this if the package has a setup.py. This macro ensures
 25 | ## modules and global scripts declared therein get installed
 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 27 | # catkin_python_setup()
 28 | 
 29 | ################################################
 30 | ## Declare ROS messages, services and actions ##
 31 | ################################################
 32 | 
 33 | ## To declare and build messages, services or actions from within this
 34 | ## package, follow these steps:
 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 36 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 37 | ## * In the file package.xml:
 38 | ##   * add a build_depend tag for "message_generation"
 39 | ##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
 40 | ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 41 | ##     but can be declared for certainty nonetheless:
 42 | ##     * add a run_depend tag for "message_runtime"
 43 | ## * In this file (CMakeLists.txt):
 44 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 45 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 46 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 47 | ##     catkin_package(CATKIN_DEPENDS ...)
 48 | ##   * uncomment the add_*_files sections below as needed
 49 | ##     and list every .msg/.srv/.action file to be processed
 50 | ##   * uncomment the generate_messages entry below
 51 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 52 | 
 53 | ## Generate messages in the 'msg' folder
 54 | # add_message_files(
 55 | #   FILES
 56 | #   Message1.msg
 57 | #   Message2.msg
 58 | # )
 59 | 
 60 | ## Generate services in the 'srv' folder
 61 | # add_service_files(
 62 | #   FILES
 63 | #   Service1.srv
 64 | #   Service2.srv
 65 | # )
 66 | 
 67 | ## Generate actions in the 'action' folder
 68 | # add_action_files(
 69 | #   FILES
 70 | #   Action1.action
 71 | #   Action2.action
 72 | # )
 73 | 
 74 | ## Generate added messages and services with any dependencies listed here
 75 | # generate_messages(
 76 | #   DEPENDENCIES
 77 | #   geometry_msgs#   std_msgs#   visualization_msgs
 78 | # )
 79 | 
 80 | ################################################
 81 | ## Declare ROS dynamic reconfigure parameters ##
 82 | ################################################
 83 | 
 84 | ## To declare and build dynamic reconfigure parameters within this
 85 | ## package, follow these steps:
 86 | ## * In the file package.xml:
 87 | ##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
 88 | ## * In this file (CMakeLists.txt):
 89 | ##   * add "dynamic_reconfigure" to
 90 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 91 | ##   * uncomment the "generate_dynamic_reconfigure_options" section below
 92 | ##     and list every .cfg file to be processed
 93 | 
 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder
 95 | # generate_dynamic_reconfigure_options(
 96 | #   cfg/DynReconf1.cfg
 97 | #   cfg/DynReconf2.cfg
 98 | # )
 99 | 
100 | ###################################
101 | ## catkin specific configuration ##
102 | ###################################
103 | ## The catkin_package macro generates cmake config files for your package
104 | ## Declare things to be passed to dependent projects
105 | ## INCLUDE_DIRS: uncomment this if you package contains header files
106 | ## LIBRARIES: libraries you create in this project that dependent projects also need
107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
108 | ## DEPENDS: system dependencies of this project that dependent projects also need
109 | catkin_package(
110 | #  INCLUDE_DIRS include
111 | #  LIBRARIES blue_gazebo
112 | #  CATKIN_DEPENDS geometry_msgs interactive_markers kdl_parser roscpp std_msgs tf visualization_msgs
113 | #  DEPENDS system_lib
114 | )
115 | 
116 | ###########
117 | ## Build ##
118 | ###########
119 | 
120 | ## Specify additional locations of header files
121 | ## Your package locations should be listed before other locations
122 | include_directories(
123 | # include
124 |   ${catkin_INCLUDE_DIRS}
125 | )
126 | 
127 | ## Declare a C++ library
128 | # add_library(${PROJECT_NAME}
129 | #   src/${PROJECT_NAME}/blue_gazebo.cpp
130 | # )
131 | 
132 | ## Add cmake target dependencies of the library
133 | ## as an example, code may need to be generated before libraries
134 | ## either from message generation or dynamic reconfigure
135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
136 | 
137 | ## Declare a C++ executable
138 | ## With catkin_make all packages are built within a single CMake context
139 | ## The recommended prefix ensures that target names across packages don't collide
140 | # add_executable(${PROJECT_NAME}_node src/blue_gazebo_node.cpp)
141 | 
142 | ## Rename C++ executable without prefix
143 | ## The above recommended prefix causes long target names, the following renames the
144 | ## target back to the shorter version for ease of user use
145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
147 | 
148 | ## Add cmake target dependencies of the executable
149 | ## same as for the library above
150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
151 | 
152 | ## Specify libraries to link a library or executable target against
153 | # target_link_libraries(${PROJECT_NAME}_node
154 | #   ${catkin_LIBRARIES}
155 | # )
156 | 
157 | #############
158 | ## Install ##
159 | #############
160 | 
161 | # all install targets should use catkin DESTINATION variables
162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
163 | 
164 | ## Mark executable scripts (Python etc.) for installation
165 | ## in contrast to setup.py, you can choose the destination
166 | # install(PROGRAMS
167 | #   scripts/my_python_script
168 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
169 | # )
170 | 
171 | ## Mark executables and/or libraries for installation
172 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
173 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
174 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
175 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
176 | # )
177 | 
178 | ## Mark cpp header files for installation
179 | # install(DIRECTORY include/${PROJECT_NAME}/
180 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
181 | #   FILES_MATCHING PATTERN "*.h"
182 | #   PATTERN ".svn" EXCLUDE
183 | # )
184 | 
185 | ## Mark other files for installation (e.g. launch and bag files, etc.)
186 | # install(FILES
187 | #   # myfile1
188 | #   # myfile2
189 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
190 | # )
191 | 
192 | #############
193 | ## Testing ##
194 | #############
195 | 
196 | ## Add gtest based cpp test target and link libraries
197 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_blue_gazebo.cpp)
198 | # if(TARGET ${PROJECT_NAME}-test)
199 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
200 | # endif()
201 | 
202 | ## Add folders to be run by python nosetests
203 | # catkin_add_nosetests(test)
204 | 
205 | 
206 | # add_executable(pose_target_simulation src/pose_target_simulation.cpp)
207 | # target_link_libraries(pose_target_simulation ${catkin_LIBRARIES})
208 | 


--------------------------------------------------------------------------------