-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrrbot3.xacro
118 lines (100 loc) · 3.6 KB
/
rrbot3.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
<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="width" value="0.1" /> <!-- Beams are square in length and width -->
<xacro:property name="height1" value="2" /> <!-- Link 1 -->
<xacro:property name="height2" value="1" /> <!-- Link 2 -->
<xacro:property name="height3" value="1" /> <!-- Link 3 -->
<xacro:property name="axle_offset" value="0.05" /> <!-- Space between joint and end of beam -->
<xacro:property name="damp" value="0.7" /> <!-- damping coefficient -->
<!-- Default Inertial -->
<xacro:macro name="default_inertial" params="z_value i_value mass">
<inertial>
<origin xyz="0 0 ${z_value}" rpy="0 0 0"/>
<mass value="${mass}" />
<inertia ixx="${i_value}" ixy="0.0" ixz="0.0"
iyy="${i_value}" iyz="0.0"
izz="${i_value}" />
</inertial>
</xacro:macro>
<!-- Import Rviz colors -->
<xacro:include filename="$(find rrbot)/urdf/materials.xacro" />
<!-- Import Gazebo elements, including Gazebo colors -->
<xacro:include filename="$(find rrbot)/urdf/rrbot.gazebo" />
<!-- Used for fixing rrbot frame to Gazebo world frame -->
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="base_link"/>
</joint>
<!-- Base Link -->
<link name="base_link">
<visual>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
</collision>
<xacro:default_inertial z_value="${height1/2}" i_value="1.0" mass="1"/>
</link>
<!-- Joint between Base Link and Middle Link -->
<joint name="joint_base_mid" type="revolute">
<parent link="base_link"/>
<child link="mid_link"/>
<origin xyz="0 ${width} ${height1 - axle_offset}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damp}"/>
<limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
</joint>
<!-- Middle Link -->
<link name="mid_link">
<visual>
<origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<origin xyz="0 0 ${height2/2 - axle_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
</collision>
<xacro:default_inertial z_value="${height2/2 - axle_offset}" i_value="1.0" mass="1"/>
</link>
<!-- Joint between Middle Link and Top Link -->
<joint name="joint_mid_top" type="revolute">
<parent link="mid_link"/>
<child link="top_link"/>
<origin xyz="0 ${width} ${height2 - axle_offset*2}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="${damp}"/>
<limit effort="100.0" velocity="0.5" lower="-3.14" upper="3.14" />
</joint>
<!-- Top Link -->
<link name="top_link">
<visual>
<origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height3}"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<origin xyz="0 0 ${height3/2 - axle_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height3}"/>
</geometry>
</collision>
<xacro:default_inertial z_value="${height3/2 - axle_offset}" i_value="1.0" mass="1"/>
</link>
</robot>