-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathtiago_dual.urdf.xacro
170 lines (151 loc) · 10.2 KB
/
tiago_dual.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
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
<?xml version="1.0"?>
<!--
Copyright (c) 2019, PAL Robotics, S.L.
All rights reserved.
This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro"
name="tiago_dual" >
<xacro:arg name="arm_left" default="true"/> <!-- true, false -->
<xacro:arg name="arm_right" default="true"/> <!-- true, false -->
<xacro:arg name="wrist_left_model" default="wrist-2010"/>
<xacro:arg name="wrist_right_model" default="wrist-2010"/>
<xacro:arg name="end_effector_left" default="pal-hey5"/> <!-- pal-gripper, pal-hey5, schunk-wsg, custom-->
<xacro:arg name="end_effector_right" default="pal-hey5"/> <!-- pal-gripper, pal-hey5, schunk-wsg, custom-->
<xacro:arg name="ft_sensor_left" default="schunk-ft"/> <!-- false, schunk-ft -->
<xacro:arg name="ft_sensor_right" default="schunk-ft"/> <!-- false, schunk-ft -->
<xacro:arg name="laser_model" default="sick-571"/> <!-- false, hokuyo, sick-551, sick-561, sick-571-->
<xacro:arg name="camera_model" default="orbbec-astra"/> <!-- false, orbbec-astra, orbbec-astra-pro, asus-xtion -->
<xacro:arg name="has_screen" default="false"/> <!-- true, false -->
<xacro:arg name="multiple" default="false"/>
<xacro:arg name="base_type" default="pmb2"/> <!-- pmb2, omni_base -->
<xacro:arg name="description_calibration_dir" default="$(find tiago_dual_description_calibration)/urdf/calibration"/>
<xacro:arg name="extrinsic_calibration_dir" default="$(find tiago_dual_description_calibration)/urdf/calibration"/>
<xacro:arg name="namespace" default=""/>
<xacro:arg name="fixed_torso" default="false" />
<xacro:arg name="no_safety_eps" default="false"/>
<xacro:arg name="gazebo_control_period" default="0.001"/> <!-- 1000Hz or 100 Hz-->
<xacro:property name="wrist_left_model" value="$(arg wrist_left_model)" />
<xacro:property name="wrist_right_model" value="$(arg wrist_right_model)" />
<xacro:property name="has_arm_left" value="$(arg arm_left)" />
<xacro:property name="has_arm_right" value="$(arg arm_right)" />
<xacro:property name="end_effector_left" value="$(arg end_effector_left)" />
<xacro:property name="end_effector_right" value="$(arg end_effector_right)" />
<xacro:property name="ft_sensor_left" value="$(arg ft_sensor_left)" />
<xacro:property name="ft_sensor_right" value="$(arg ft_sensor_right)" />
<xacro:property name="laser_model" value="$(arg laser_model)" />
<xacro:property name="camera_model" value="$(arg camera_model)" />
<xacro:property name="has_screen" value="$(arg has_screen)" />
<xacro:property name="base_type" value="$(arg base_type)" />
<xacro:property name="is_multiple" value="$(arg multiple)" />
<xacro:property name="nsp" value="$(arg namespace)" />
<xacro:property name="no_safety_eps" value="$(arg no_safety_eps)" />
<xacro:property name="fixed_torso" value="$(arg fixed_torso)" />
<xacro:property name="gazebo_control_period" value="$(arg gazebo_control_period)" />
<xacro:if value="${wrist_left_model not in ['wrist-2010', 'wrist-2017']}">
<xacro:wrong_wrist_model/>
</xacro:if>
<xacro:if value="${wrist_right_model not in ['wrist-2010', 'wrist-2017']}">
<xacro:wrong_wrist_model/>
</xacro:if>
<xacro:if value="${end_effector_left not in ['pal-gripper', 'pal-hey5', 'schunk-wsg', 'robotiq-2f-85', 'robotiq-2f-140', 'robotiq-epick', 'false', False, 'custom', 'no-ee']}">
<xacro:wrong_end_effector_left/>
</xacro:if>
<xacro:if value="${end_effector_right not in ['pal-gripper', 'pal-hey5', 'schunk-wsg', 'robotiq-2f-85', 'robotiq-2f-140', 'robotiq-epick', 'false', False, 'custom', 'no-ee']}">
<xacro:wrong_end_effector_right/>
</xacro:if>
<xacro:if value="${ft_sensor_left not in ['false', False, 'schunk-ft']}">
<xacro:wrong_ft_sensor_left/>
</xacro:if>
<xacro:if value="${ft_sensor_right not in ['false', False, 'schunk-ft']}">
<xacro:wrong_ft_sensor_right/>
</xacro:if>
<xacro:if value="${laser_model not in ['false', False, 'sick-551', 'sick-561', 'sick-571', 'hokuyo', 'ydlidar-tg30', 'ydlidar-tg15']}">
<xacro:wrong_laser_model/>
</xacro:if>
<xacro:if value="${camera_model not in ['false', False, 'orbbec-astra', 'orbbec-astra-pro', 'asus-xtion']}">
<xacro:wrong_camera_model/>
</xacro:if>
<xacro:if value="${base_type not in ['pmb2', 'omni_base']}">
<xacro:wrong_base_type/>
</xacro:if>
<xacro:property name="has_schunk_ft" value="${ft_sensor == 'schunk-ft'}"/>
<xacro:property name="has_ft_sensor_left" value="${ft_sensor_left == 'schunk-ft'}"/> <!-- This should be OR'ed with other FT sensors-->
<xacro:property name="has_ft_sensor_right" value="${ft_sensor_right == 'schunk-ft'}"/> <!-- This should be OR'ed with other FT sensors-->
<xacro:property name="has_end_effector_left" value="${end_effector_left not in ['false', 'False', 'no-ee']}"/>
<xacro:property name="end_effector_left_link" value="${'wrist_left_ft_tool_link' if has_ft_sensor_left else 'arm_left_tool_link'}"/>
<xacro:property name="end_effector_left_name" value="${'hand_left' if end_effector_left == 'pal-hey5' else 'gripper_left'}"/>
<xacro:property name="has_end_effector_right" value="${end_effector_right not in ['false', 'False', 'no-ee']}"/>
<xacro:property name="end_effector_right_link" value="${'wrist_right_ft_tool_link' if has_ft_sensor_right else 'arm_right_tool_link'}"/>
<xacro:property name="end_effector_right_name" value="${'hand_right' if end_effector_right == 'pal-hey5' else 'gripper_right'}"/>
<xacro:property name="head_link_name" value="head_2_link"/>
<xacro:property name="description_calibration_dir" value="${arg description_calibration_dir}"/>
<!-- The following included files set up definitions of parts of the robot body -->
<!--File includes-->
<xacro:include filename="$(find pal_urdf_utils)/urdf/deg_to_rad.xacro" />
<!-- Base -->
<xacro:include filename="$(find ${base_type}_description)/urdf/base/base_sensors.urdf.xacro" />
<!-- Torso -->
<xacro:include filename="$(find tiago_dual_description)/urdf/torso/torso_dual.urdf.xacro" />
<!-- Arm -->
<xacro:include filename="$(arg description_calibration_dir)/calibration_constants.urdf.xacro" />
<xacro:include filename="$(find tiago_description)/urdf/arm/arm.urdf.xacro" />
<!-- End Effector -->
<xacro:include filename="$(find tiago_description)/urdf/end_effector/end_effector.urdf.xacro" />
<!-- Force Torque sensor -->
<xacro:include filename="$(find tiago_description)/urdf/sensors/ftsensor.urdf.xacro" />
<!-- Head -->
<xacro:include filename="$(find tiago_description)/urdf/head/head.urdf.xacro" />
<!-- Materials for visualization -->
<xacro:include filename="$(find pal_urdf_utils)/urdf/materials.urdf.xacro" />
<!-- Generic simulator_gazebo plugins -->
<xacro:include filename="$(find tiago_description)/gazebo/gazebo.urdf.xacro" />
<!-- Now we can start using the macros included above to define the actual robot -->
<xacro:if value="${base_type == 'pmb2'}">
<xacro:base_sensors name="base" laser_model="$(arg laser_model)" sonars="true" microphone="true"/>
</xacro:if>
<xacro:if value="${base_type == 'omni_base'}">
<xacro:base_sensors name="base" front_laser_model="$(arg laser_model)" rear_laser_model="$(arg laser_model)"/>
</xacro:if>
<xacro:tiago_dual_torso name="torso" parent="base_link" has_screen="$(arg has_screen)"/>
<xacro:head name="head" parent="torso_lift_link" camera_model="$(arg camera_model)"
description_calibration_dir="$(arg description_calibration_dir)"
extrinsic_calibration_dir="$(arg extrinsic_calibration_dir)"
no_safety_eps="${no_safety_eps}"/>
<!-- Left -->
<xacro:if value="${has_arm_left}">
<xacro:tiago_arm name="arm_left" parent="torso_lift_link" wrist_model="${wrist_left_model}" has_ft_sensor="${has_ft_sensor_left}" reflect="-1" tiago_dual="1" arm_1_offset="${arm_left_1_joint_offset}" arm_2_offset="${arm_left_2_joint_offset}" arm_3_offset="${arm_left_3_joint_offset}" arm_4_offset="${arm_left_4_joint_offset}" arm_5_offset="${arm_left_5_joint_offset}" arm_6_offset="${arm_left_6_joint_offset}" arm_7_offset="${arm_left_7_joint_offset}" no_safety_eps="${no_safety_eps}">
<origin xyz="0.02556 0.19 -0.171" rpy="${180 * deg_to_rad} 0 ${90 * deg_to_rad}"/>
</xacro:tiago_arm>
<xacro:if value="${has_ft_sensor_left}">
<xacro:ft_sensor name="wrist_left" parent="arm_left_tool_link" />
</xacro:if>
<xacro:tiago_end_effector name="${end_effector_left_name}" parent="${end_effector_left_link}" type="${end_effector_left}"
has_end_effector="${has_end_effector_left}" reflect="-1" is_dual="true"/>
</xacro:if>
<!-- Right -->
<xacro:if value="${has_arm_right}">
<xacro:tiago_arm name="arm_right" parent="torso_lift_link" wrist_model="${wrist_right_model}" has_ft_sensor="${has_ft_sensor_right}" reflect="1" tiago_dual="1" arm_1_offset="${arm_right_1_joint_offset}" arm_2_offset="${arm_right_2_joint_offset}" arm_3_offset="${arm_right_3_joint_offset}" arm_4_offset="${arm_right_4_joint_offset}" arm_5_offset="${arm_right_5_joint_offset}" arm_6_offset="${arm_right_6_joint_offset}" arm_7_offset="${arm_right_7_joint_offset}" no_safety_eps="${no_safety_eps}">
<origin xyz="0.02556 -0.19 -0.171" rpy="0 0 ${-90 * deg_to_rad}"/>
</xacro:tiago_arm>
<xacro:if value="${has_ft_sensor_right}">
<xacro:ft_sensor name="wrist_right" parent="arm_right_tool_link" />
</xacro:if>
<xacro:tiago_end_effector name="${end_effector_right_name}" parent="${end_effector_right_link}" type="${end_effector_right}" has_end_effector="${has_end_effector_right}" reflect="1" is_dual="true"/>
</xacro:if>
<!-- RGBD Laser Link -->
<joint name="rgbd_laser_joint" type="fixed">
<parent link="base_footprint"/>
<child link="rgbd_laser_link"/>
<origin xyz="-0.9 0 0" rpy="0 0 0" />
</joint>
<link name="rgbd_laser_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.0001" />
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
</inertial>
</link>
</robot>