mirror of
https://github.com/6-robot/wpb_home.git
synced 2025-09-15 12:58:59 +08:00
232 lines
6.1 KiB
XML
232 lines
6.1 KiB
XML
<?xml version="1.0"?>
|
|
<robot name="wp_v4">
|
|
|
|
<!-- material -->
|
|
<material name = "black">
|
|
<color rgba = "0.01 0.01 0.01 1"/>
|
|
</material>
|
|
<material name = "grey">
|
|
<color rgba = "0.2 0.2 0.2 1"/>
|
|
</material>
|
|
<material name = "blue">
|
|
<color rgba = "0.2 0.2 0.9 1"/>
|
|
</material>
|
|
|
|
|
|
<link name="base_footprint">
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.05 0.05 0.001" />
|
|
</geometry>
|
|
<material name="TransparentGreen" />
|
|
</visual>
|
|
|
|
</link>
|
|
|
|
<joint name="base_joint" type="fixed">
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<parent link="base_footprint"/>
|
|
<child link="base_link" />
|
|
</joint>
|
|
|
|
<!-- base -->
|
|
<link name="base_link">
|
|
<visual>
|
|
<geometry>
|
|
<cylinder length="0.6" radius="0.0"/>
|
|
</geometry>
|
|
<origin rpy = "0 0 0" xyz = "0 0 0"/>
|
|
</visual>
|
|
</link>
|
|
|
|
<!-- body -->
|
|
<link name = "body_link">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://wpb_home_bringup/meshes/wpb_home_mani.dae" scale=" 1 1 1 "/>
|
|
</geometry>
|
|
<origin rpy = "1.57 0 1.57" xyz = "-.225 -0.225 0"/>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz=".013 0 .68" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="1.37" radius="0.226"/>
|
|
</geometry>
|
|
</collision>
|
|
<inertial>
|
|
<mass value="10"/>
|
|
<inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
|
|
</inertial>
|
|
</link>
|
|
<joint name = "base_to_body" type = "fixed">
|
|
<parent link = "base_link"/>
|
|
<child link = "body_link"/>
|
|
<origin rpy="0 0 0" xyz="0 0 0"/> <!--pos-->
|
|
</joint>
|
|
<gazebo reference = "body_link">
|
|
<turnGravityOff>false</turnGravityOff>
|
|
</gazebo>
|
|
|
|
<!-- lidar -->
|
|
<link name = "laser">
|
|
<visual>
|
|
<geometry>
|
|
<cylinder length="0.00" radius="0.00"/>
|
|
</geometry>
|
|
<origin rpy = "0 0 0" xyz = "0 0 0"/>
|
|
</visual>
|
|
</link>
|
|
<joint name="laser_joint" type="fixed">
|
|
<origin rpy="0 0 0" xyz="0 0 0" /> <!--pos-->
|
|
<parent link="base_link" />
|
|
<child link="laser" />
|
|
</joint>
|
|
|
|
<!-- kinect -->
|
|
<link name = "kinect2_dock">
|
|
<visual>
|
|
<geometry>
|
|
<!-- <box size=".01 .25 .07"/>-->
|
|
<box size="0 0 0"/>
|
|
</geometry>
|
|
<origin rpy = "0 0 0" xyz = "0 0 0"/>
|
|
<material name = "red"/>
|
|
</visual>
|
|
</link>
|
|
<joint name="kinect_height" type="prismatic">
|
|
<parent link="base_link"/>
|
|
<child link="kinect2_dock"/>
|
|
<limit effort="1000.0" lower="0" upper="1.7" velocity="0.5"/>
|
|
<origin xyz="0.145 -0.013 0" rpy="0 -1.5707963 0"/>
|
|
</joint>
|
|
|
|
<link name = "kinect2_ir_optical_frame">
|
|
<visual>
|
|
<geometry>
|
|
<!-- <box size=".25 .04 .07"/>-->
|
|
<box size="0 0 0"/>
|
|
</geometry>
|
|
<origin xyz = "0 0 0" rpy = "0 0 0"/>
|
|
<material name = "blue"/>
|
|
</visual>
|
|
</link>
|
|
|
|
<joint name="kinect_pitch" type="revolute">
|
|
<axis xyz="1 0 0"/>
|
|
<limit effort="1000.0" lower="-1.57" upper="1.57" velocity="0.5"/>
|
|
<origin xyz="0 0 0" rpy="3.1415926 0 -1.5707963" /> <!--pos-->
|
|
<parent link="kinect2_dock" />
|
|
<child link="kinect2_ir_optical_frame" />
|
|
</joint>
|
|
<!-- <joint name="kinect_joint" type="fixed">
|
|
<origin xyz="0.174 0 1.35" rpy="-1.5707963 0 -1.5707963" />
|
|
<parent link="base_link" />
|
|
<child link="kinect2_ir_optical_frame" />
|
|
</joint>-->
|
|
|
|
<link name = "kinect2_rgb_optical_frame">
|
|
<visual>
|
|
<geometry>
|
|
<box size="0 0 0"/>
|
|
</geometry>
|
|
<origin rpy = "0 0 0" xyz = "0 0 0"/>
|
|
<material name = "blue"/>
|
|
</visual>
|
|
</link>
|
|
<joint name="kinect_hd_joint" type="fixed">
|
|
<origin xyz="0 0 0" rpy="0 0 0" /> <!--pos-->
|
|
<parent link="kinect2_ir_optical_frame" />
|
|
<child link="kinect2_rgb_optical_frame" />
|
|
</joint>
|
|
|
|
<!-- manipulator -->
|
|
<link name = "mani_lift">
|
|
<visual>
|
|
<geometry>
|
|
<box size=".03 .04 .06"/>
|
|
</geometry>
|
|
<origin rpy = "0 0 0" xyz = "0 0 -0.06"/>
|
|
<material name = "black"/>
|
|
</visual>
|
|
</link>
|
|
<joint name="mani_base" type="prismatic">
|
|
<parent link="base_link"/>
|
|
<child link="mani_lift"/>
|
|
<limit effort="1000.0" lower="0" upper="0.7" velocity="0.5"/>
|
|
<origin xyz="0.1 0 0.4" rpy="0 -1.5707963 0"/>
|
|
</joint>
|
|
|
|
<link name = "mani_elbow">
|
|
<visual>
|
|
<geometry>
|
|
<cylinder length="0.00" radius="0.00"/>
|
|
</geometry>
|
|
<origin rpy = "0 0 0" xyz = "0 0 0"/>
|
|
</visual>
|
|
</link>
|
|
<joint name="lift_elbow" type="fixed">
|
|
<origin rpy="0 0 0" xyz="0 0 -0.08" /> <!--pos-->
|
|
<parent link="mani_lift" />
|
|
<child link="mani_elbow" />
|
|
</joint>
|
|
|
|
<link name = "mani_forearm">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://wpb_home_bringup/meshes/forearm.dae" scale=" 1 1 1 "/>
|
|
</geometry>
|
|
<origin rpy = "-1.5707963 3.1415926 0" xyz = "0.365 -0.037 -0.0355"/>
|
|
</visual>
|
|
</link>
|
|
|
|
<joint name="elbow_forearm" type="revolute">
|
|
<axis xyz="0 1 0"/>
|
|
<limit effort="1000.0" lower="0" upper="1.57" velocity="0.5"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" /> <!--pos-->
|
|
<parent link="mani_elbow" />
|
|
<child link="mani_forearm" />
|
|
</joint>
|
|
|
|
<!-- finger -->
|
|
<link name = "mani_left_finger">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://wpb_home_bringup/meshes/finger.dae" scale=" 1 1 1 "/>
|
|
</geometry>
|
|
<origin rpy = "-1.57 0 1.57" xyz = "0.154 -0.005 0.009"/>
|
|
</visual>
|
|
</link>
|
|
<joint name = "forearm_left_finger" type = "revolute">
|
|
<parent link = "mani_forearm"/>
|
|
<child link = "mani_left_finger"/>
|
|
<origin xyz = "0.32 0.0125 0"/>
|
|
<axis xyz = "0 0 1"/>
|
|
<limit effort = "1000.0" lower="0" upper="1.0" velocity="0.5"/>
|
|
</joint>
|
|
|
|
<link name = "mani_right_finger">
|
|
<visual>
|
|
<geometry>
|
|
<mesh filename="package://wpb_home_bringup/meshes/finger.dae" scale="1 1 1"/>
|
|
</geometry>
|
|
<origin rpy = "1.57 0 -1.57" xyz = "0.154 0.005 -0.020"/>
|
|
</visual>
|
|
</link>
|
|
<joint name = "forearm_right_finger" type = "revolute">
|
|
<parent link = "mani_forearm"/>
|
|
<child link = "mani_right_finger"/>
|
|
<origin xyz = "0.32 -0.0125 0"/>
|
|
<axis xyz = "0 0 -1"/>
|
|
<limit effort = "1000.0" lower="0" upper="1.0" velocity="0.5"/>
|
|
</joint>
|
|
|
|
<!-- Gazebo plugin for ROS Control -->
|
|
<gazebo>
|
|
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
|
<robotNamespace>/</robotNamespace>
|
|
</plugin>
|
|
</gazebo>
|
|
|
|
</robot>
|