feat: add table and initial position params

This commit is contained in:
Ilya Uraev 2025-06-03 00:02:23 +03:00
parent c82466ed4a
commit a10ded747a
3 changed files with 130 additions and 13 deletions

View file

@ -11,6 +11,7 @@
<xacro:include filename="$(find dh_ag95_description)/urdf/dh_ag95_macro.urdf.xacro" />
<xacro:include filename="$(find dh_ag145_description)/urdf/dh_ag145_macro.urdf.xacro" />
<xacro:include filename="$(find aubo_description)/urdf/sensors.xacro" />
<xacro:include filename="$(find simple_gz_gripper)/urdf/simple_gripper_macro.xacro"/>
<xacro:property name="hardware" value="$(arg hardware)"/>
<xacro:property name="robot_ip" value="$(arg robot_ip)"/>
@ -242,11 +243,12 @@
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</joint>
<!-- <xacro:if value="${with_gripper=='true'}"> -->
<xacro:dh_ag145_gripper prefix="" parent="wrist3_Link">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
</xacro:dh_ag145_gripper>
<!-- </xacro:if> -->
<!-- <xacro:dh_ag145_gripper prefix="" parent="wrist3_Link"> -->
<!-- <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /> -->
<!-- </xacro:dh_ag145_gripper> -->
<xacro:simple_parallel_gripper prefix="" parent="wrist3_Link">
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 1.57" />
</xacro:simple_parallel_gripper>
<xacro:rgbd parent="wrist3_Link" tf_prefix="">

View file

@ -4,7 +4,7 @@
<ros2_control name="aubo_hardware_interface" type="system">
<hardware>
<xacro:if value="${hardware=='gazebo'}">
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
</xacro:if>
<xacro:if value="${hardware=='mock'}">
<plugin>mock_components/GenericSystem</plugin>
@ -19,7 +19,9 @@
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity" />
</joint>
@ -28,7 +30,14 @@
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position" />
<xacro:if value="${hardware=='gazebo'}">
<state_interface name="position">
<param name="initial_value">0.5</param>
</state_interface>
</xacro:if>
<xacro:if value="${hardware=='real' or hardware=='mock'}">
<state_interface name="position" />
</xacro:if>
<state_interface name="velocity" />
</joint>
@ -37,7 +46,14 @@
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position" />
<xacro:if value="${hardware=='gazebo'}">
<state_interface name="position">
<param name="initial_value">1.75</param>
</state_interface>
</xacro:if>
<xacro:if value="${hardware=='real' or hardware=='mock'}">
<state_interface name="position" />
</xacro:if>
<state_interface name="velocity" />
</joint>
@ -45,7 +61,14 @@
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position" />
<xacro:if value="${hardware=='gazebo'}">
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
</xacro:if>
<xacro:if value="${hardware=='real' or hardware=='mock'}">
<state_interface name="position" />
</xacro:if>
<state_interface name="velocity" />
</joint>
@ -53,7 +76,14 @@
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position" />
<xacro:if value="${hardware=='gazebo'}">
<state_interface name="position">
<param name="initial_value">1.57</param>
</state_interface>
</xacro:if>
<xacro:if value="${hardware=='real' or hardware=='mock'}">
<state_interface name="position" />
</xacro:if>
<state_interface name="velocity" />
</joint>
@ -61,7 +91,14 @@
<command_interface name="position" />
<command_interface name="velocity" />
<state_interface name="position" />
<xacro:if value="${hardware=='gazebo'}">
<state_interface name="position">
<param name="initial_value">0.0</param>
</state_interface>
</xacro:if>
<xacro:if value="${hardware=='real' or hardware=='mock'}">
<state_interface name="position" />
</xacro:if>
<state_interface name="velocity" />
</joint>

View file

@ -1,4 +1,4 @@
<?xml version="1.0" ?>
<?xml version="1.0"?>
<sdf version="1.6">
<world name="aubo-world">
<physics name="1ms" type="dart">
@ -66,5 +66,83 @@
</link>
</model>
<model name="table">
<static>true</static>
<pose>0.5 0 0.1 0 0 0</pose>
<link name="cylinder_link">
<inertial auto="true" />
<collision name="table_collision">
<geometry>
<box>
<size>0.3 0.3 0.2</size>
</box>
</geometry>
</collision>
<visual name="table_visual">
<geometry>
<box>
<size>0.3 0.3 0.2</size>
</box>
</geometry>
<material>
<ambient>0.6 0.3 0.3 1</ambient>
<diffuse>0.6 0.3 0.3 1</diffuse>
</material>
</visual>
</link>
</model>
<model name="simple_cylinder">
<pose>0.5 0 0.252 0 0 0</pose>
<link name="cylinder_link">
<inertial>
<mass>0.05</mass>
<pose>0 0 0 0 0 0</pose>
<inertia>
<ixx>3.2083e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>3.2083e-05</iyy>
<iyz>0</iyz>
<izz>1.0e-05</izz>
</inertia>
</inertial>
<collision name="cylinder_collision">
<geometry>
<cylinder>
<radius>0.03</radius>
<length>0.1</length>
</cylinder>
</geometry>
<surface>
<contact>
<ode>
<min_depth>0.001</min_depth>
<max_vel>0.01</max_vel>
</ode>
</contact>
<friction>
<ode>
<mu>10.0</mu>
<mu2>10.0</mu2>
</ode>
</friction>
</surface>
</collision>
<visual name="cylinder_visual">
<geometry>
<cylinder>
<radius>0.03</radius>
<length>0.1</length>
</cylinder>
</geometry>
<material>
<ambient>0.3 0.3 0.6 1</ambient>
<diffuse>0.3 0.3 0.6 1</diffuse>
</material>
</visual>
</link>
</model>
</world>
</sdf>