-
Notifications
You must be signed in to change notification settings - Fork 0
/
gazebo.launch
103 lines (94 loc) · 3.82 KB
/
gazebo.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
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
<launch>
<arg name="plank_of_wood_id" default="1" />
<!-- Load the environment -->
<group ns="gazebo">
<include file="$(find gerard_bauzil)/launch/plank_of_wood.launch">
<arg name="prefix" value="box" />
<arg name="id" value="$(arg plank_of_wood_id)" />
</include>
<include file="$(find gerard_bauzil)/launch/rolling_table.launch">
<arg name="prefix" value="table" />
</include>
<!-- xyzrpy of plank_of_wood on table -->
<!---x -0.075 -z 0.024 -z 1.168 -R 4.712 -P 1.57-->
<node name="spawn_plank_of_wood" pkg="gazebo_ros" type="spawn_model"
args="-file $(find gerard_bauzil)/urdf/plank_of_wood$(arg plank_of_wood_id).urdf -urdf
-x 0.45891797741593393 -y -0.25 -z 0.832 -R 1.57 -P 1.57 -Y 0
-model box" />
<node name="spawn_table" pkg="gazebo_ros" type="spawn_model"
args="-file $(find gerard_bauzil)/urdf/rolling_table.urdf -urdf
-x 0. -y 0. -z 0. -Y 0.
-model table" />
<node name="box_to_tf" pkg="agimus_demos" type="gazebo_object_to_tf.py"
args="box base_link world" required="true"
/>
<node name="table_to_tf" pkg="agimus_demos" type="gazebo_object_to_tf.py"
args="table base_link world" required="true"
/>
<node pkg="tf" type="static_transform_publisher"
name="odom_to_world"
args="0 0 0 0 0 0 1 world odom 100" />
</group>
<!-- Which robot are we controlling ? -->
<arg name="script_file" doc="Full path to the script which initialize the supervisor"
default="$(find agimus_demos)/talos/manipulate_boxes/supervisor.py" />
<include file="$(find talos_gazebo)/launch/talos_gazebo.launch" >
<!-- Gazebo args:
-u: paused
-->
<arg name="extra_gazebo_args" value="-u"/>
<!---x -0.6340 -y -0.2027 -z 1.05 -R -0.034 -P 0.027 -Y 0.125-->
<!---x -0.6 -y -0.2 -z 1.0192720229567027-->
<arg name="gzpose" value="
-x 0.5402763680625408 -y -0.833196863501999 -z 1.0199316910041052
-R -0.02265708 -P 0.06454163 -Y 1.63636329
-J arm_left_1_joint 0.25847
-J arm_left_2_joint 0.173046
-J arm_left_3_joint -0.0002
-J arm_left_4_joint -0.525366
-J arm_left_5_joint 0.0
-J arm_left_6_joint 0.0
-J arm_left_7_joint 0.1
-J arm_right_1_joint -0.25847
-J arm_right_2_joint -0.173046
-J arm_right_3_joint 0.0002
-J arm_right_4_joint -0.525366
-J arm_right_5_joint 0.0
-J arm_right_6_joint 0.0
-J arm_right_7_joint 0.1
-J gripper_left_fingertip_1_joint 0.0
-J gripper_left_fingertip_2_joint 0.0
-J gripper_left_fingertip_3_joint 0.0
-J gripper_left_inner_double_joint 0.0
-J gripper_left_inner_single_joint 0.0
-J gripper_left_joint 0.0
-J gripper_left_motor_single_joint 0.0
-J gripper_right_fingertip_1_joint 0.0
-J gripper_right_fingertip_2_joint 0.0
-J gripper_right_fingertip_3_joint 0.0
-J gripper_right_inner_double_joint 0.0
-J gripper_right_inner_single_joint 0.0
-J gripper_right_joint 0.0
-J gripper_right_motor_single_joint 0.0
-J head_1_joint 0.0
-J head_2_joint 0.0
-J leg_left_1_joint 0.0
-J leg_left_2_joint 0.0
-J leg_left_3_joint -0.411354
-J leg_left_4_joint 0.859395
-J leg_left_5_joint -0.448041
-J leg_left_6_joint -0.001708
-J leg_right_1_joint 0.0
-J leg_right_2_joint 0.0
-J leg_right_3_joint -0.411354
-J leg_right_4_joint 0.859395
-J leg_right_5_joint -0.448041
-J leg_right_6_joint -0.001708
-J torso_1_joint 0.0
-J torso_2_joint 0.006761
"/>
<!---0.017683184544140064, 0.012418080940971276, 0.06250270795619342, 0.997810857702132,-->
</include>
<include file="$(find roscontrol_sot_talos)/launch/sot_talos_controller_gazebo.launch" >
</include>
</launch>