-
Notifications
You must be signed in to change notification settings - Fork 0
/
mobile_base.xacro
101 lines (84 loc) · 3.47 KB
/
mobile_base.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
<!-- <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> -->
<xacro:property name="base_length" value="0.6" />
<xacro:property name="base_width" value="0.4" />
<xacro:property name="base_height" value="0.2" />
<xacro:property name="wheel_radius" value="0.1" />
<xacro:property name="wheel_length" value="0.05" />
<link name="base_footprint" />
<link name="base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}" />
</geometry>
<origin xyz="0 0 ${base_height / 2.0}" rpy="0 0 0" />
<material name="blue" />
</visual>
<collision>
<geometry>
<box size="${base_length} ${base_width} ${base_height}" />
</geometry>
<origin xyz="0 0 ${base_height / 2.0}" rpy="0 0 0" />
</collision>
<xacro:box_inertia m="5.0" l="${2*base_length}" w="${2*base_width}" h="${2*base_height}"
xyz="0 0 ${base_height / 2.0}" rpy="0 0 0" />
</link>
<xacro:macro name="wheel_link" params="prefix">
<link name="${prefix}_wheel_link">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
<material name="grey" />
</visual>
<collision>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_length}" />
</geometry>
<origin xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
</collision>
<xacro:cylinder_inertia m="1.0" r="${2*wheel_radius}" h="${2*wheel_length}" xyz="0 0 0" rpy="${pi / 2.0} 0 0" />
</link>
</xacro:macro>
<xacro:wheel_link prefix="right" />
<xacro:wheel_link prefix="left" />
<joint name="base_right_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="right_wheel_link" />
<origin xyz="-${base_length/ 4.0} ${-(base_width + wheel_length)/2.0} 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<joint name="base_left_wheel_joint" type="continuous">
<parent link="base_link" />
<child link="left_wheel_link" />
<origin xyz="-${base_length/ 4.0} ${(base_width + wheel_length) / 2.0} 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<link name="caster_wheel_link">
<visual>
<geometry>
<sphere radius="${wheel_radius/ 2.0}" />
</geometry>
<origin xyz= "0 0 0" rpy="0 0 0" />
<material name="grey" />
</visual>
<collision>
<geometry>
<sphere radius="${wheel_radius/ 2.0}" />
</geometry>
<origin xyz= "0 0 0" rpy="0 0 0" />
</collision>
<xacro:sphere_inertia m="0.5" r="${2*wheel_radius / 2.0}" xyz="0 0 0" rpy="0 0 0" />
</link>
<joint name="base_caster_wheel_joint" type="fixed">
<parent link="base_link" />
<child link="caster_wheel_link" />
<origin xyz="${base_length/ 3.0} 0 -${wheel_radius/ 2.0}" rpy="0 0 0" />
</joint>
<joint name="base_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_link" />
<origin xyz="0 0 ${wheel_radius}" rpy="0 0 0" />
</joint>
</robot>