Skip to content
Snippets Groups Projects
Commit 8f1dc0ff authored by Galambos Mihály's avatar Galambos Mihály
Browse files

Finishing up model parameters

parent 68ca079a
No related branches found
No related tags found
No related merge requests found
......@@ -3,61 +3,64 @@
<!-- TODO
[x] Add metadata for simulation
[] Measure exact values on robot
[] Extract caster wheel parameters
[] Maybe all property should be a positive number
[] Move base_link to the rotation center - to do this move visual and collision origin
[X] Measure exact values on robot
[?] Extract caster wheel parameters
[X] Maybe all property should be a positive number
[X] Move base_link to the rotation center - to do this move visual and collision origin
-->
<!-- * CONSTANTS * -->
<!-- Base -->
<xacro:property name="base_width" value="0.31" />
<xacro:property name="base_length" value="0.42" />
<xacro:property name="base_height" value="0.18" />
<xacro:property name="base_mass" value="15" />
<!-- Base link -->
<xacro:property name="base_x_off" value="0.1135" /> <!-- 11,35 cm-->
<xacro:property name="base_z_off" value="0.043" /> <!-- 4,3 cm -->
<!-- Robot base -->
<xacro:property name="base_width" value="0.43" /> <!-- 43 cm -->
<xacro:property name="base_length" value="0.507" /> <!-- 50,7 cm -->
<xacro:property name="base_height" value="0.25" /> <!-- 25 cm -->
<!-- Backpack frame -->
<xacro:property name="frame_diameter" value="0.03" />
<xacro:property name="frame_base_x_off" value="-0.25" />
<xacro:property name="frame_base_z_off" value="-0.05" />
<xacro:property name="frame_height" value="1" />
<xacro:property name="frame_diameter" value="0.03" /> <!-- 3 cm -->
<xacro:property name="frame_base_x_gap" value="0.10" /> <!-- 10 cm -->
<xacro:property name="frame_base_z_off" value="0.11" /> <!-- 11 cm-->
<xacro:property name="frame_height" value="1.835" /> <!-- 183,5 -->
<xacro:property name="frame_width" value="${base_width + 2*frame_diameter}" />
<xacro:property name="frame_bottom_zoff" value="0.03" />
<xacro:property name="horizontal_frame_mass" value="0.03" />
<xacro:property name="vertical_frame_mass" value="0.03" />
<xacro:property name="frame_bottom_zoff" value="0.21" /> <!-- 21 cm -->
<!-- Front frame -->
<xacro:property name="caster_frame_diameter" value="0.03" />
<xacro:property name="caster_frame_x_gap" value="0.15" />
<xacro:property name="caster_frame_width" value="${base_width}" />
<xacro:property name="caster_frame_z_off" value="0.03" />
<xacro:property name="caster_frame_parallel_mass" value="0.03" />
<xacro:property name="caster_frame_cross_mass" value="0.03" />
<xacro:property name="caster_frame_diameter" value="0.03" /> <!-- 3 cm -->
<xacro:property name="caster_frame_x_gap" value="0.10" /> <!-- 10 cm -->
<xacro:property name="caster_frame_width" value="0.53" /> <!-- 53 cm -->
<xacro:property name="caster_frame_z_off" value="0.114" /> <!-- 11,4 cm -->
<!-- Drive wheels -->
<xacro:property name="wheel_radius" value="0.10" />
<xacro:property name="wheel_width" value="0.04" />
<xacro:property name="wheel_y_gap" value="0.025" />
<xacro:property name="wheel_z_off" value="-0.05" />
<xacro:property name="wheel_x_off" value="0.12" />
<xacro:property name="wheel_mass" value="0.5" />
<!-- Drive wheel -->
<xacro:property name="wheel_radius" value="0.1725" /> <!-- 17,25 cm -->
<xacro:property name="wheel_width" value="0.08" /> <!-- 8 cm -->
<xacro:property name="wheel_y_gap" value="0.028" /> <!-- 2,8 cm -->
<!-- Caster wheels -->
<xacro:property name="caster_xgap" value="0.04" />
<xacro:property name="caster_ygap" value="0.01" />
<xacro:property name="caster_mass" value="0.5" />
<!-- Caster whhel -->
<xacro:property name="caster_base_z_off" value="0.03" /> <!-- 3 cm-->
<!-- Cameras -->
<xacro:property name="front_camera_name" value="camera_1571" />
<xacro:property name="front_camera_angle" value="0" />
<xacro:property name="front_camera_gap" value="0.022128" />
<xacro:property name="front_camera_gap" value="0.016" /> <!-- 1,6 cm-->
<xacro:property name="back_camera_name" value="camera_1502" />
<xacro:property name="back_camera_angle" value="45" />
<xacro:property name="back_camera_gap" value="0.022128" />
<xacro:property name="back_camera_angle" value="0" />
<xacro:property name="back_camera_gap" value="0.015" /> <!-- 1,6 cm-->
<!-- TODO - Mass -->
<xacro:property name="base_mass" value="15" />
<xacro:property name="vertical_frame_mass" value="0.03" />
<xacro:property name="horizontal_frame_mass" value="0.03" />
<xacro:property name="caster_frame_parallel_mass" value="0.03" />
<xacro:property name="caster_frame_cross_mass" value="0.03" />
<xacro:property name="caster_mass" value="0.5" />
<xacro:property name="wheel_mass" value="0.5" />
<!-- * MACROS * -->
<!-- Wheels -->
<xacro:macro name="wheel" params="name x_reflect y_reflect">
<xacro:macro name="wheel" params="name y_reflect">
<link name="${name}_link">
<visual>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
......@@ -81,7 +84,7 @@
<joint name="${name}_joint" type="continuous">
<parent link="base_link" />
<child link="${name}_link" />
<origin xyz="${x_reflect*wheel_x_off} ${y_reflect*(base_width/2+wheel_y_gap)} ${wheel_z_off}" rpy="0 0 0" />
<origin xyz="0.0 ${y_reflect*(base_width/2+wheel_y_gap)} 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
</xacro:macro>
......@@ -91,7 +94,7 @@
<link name="${name}_link">
<visual>
<geometry>
<sphere radius="${(wheel_radius-wheel_z_off+caster_frame_z_off-caster_frame_diameter/2)/2}" />
<sphere radius="${(wheel_radius+base_z_off+caster_frame_z_off-caster_frame_diameter/2)/2-caster_base_z_off}" />
</geometry>
<material name="Gray">
<color rgba="0.5 0.5 0.5 1.0" />
......@@ -100,17 +103,17 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<sphere radius="${(wheel_radius-wheel_z_off+caster_frame_z_off-caster_frame_diameter/2)/2}" />
<sphere radius="${(wheel_radius+base_z_off+caster_frame_z_off-caster_frame_diameter/2)/2-caster_base_z_off}" />
</geometry>
</collision>
<xacro:sphere_inertia m="${caster_mass}" r="${(wheel_radius-wheel_z_off+caster_frame_z_off-caster_frame_diameter/2)/2}" />
<xacro:sphere_inertia m="${caster_mass}" r="${(wheel_radius+base_z_off+caster_frame_z_off-caster_frame_diameter/2)/2-caster_base_z_off}" />
</link>
<joint name="${name}_joint" type="fixed">
<parent link="caster_frame_cross_link" />
<child link="${name}_link" />
<origin xyz="0.0 ${y_reflect*(caster_frame_width/2)} ${-((wheel_radius-wheel_z_off+caster_frame_z_off+caster_frame_diameter/2)/2)}" rpy="0 0 0" />
<origin xyz="${caster_frame_diameter} ${y_reflect*(caster_frame_width/2)} ${-((wheel_radius+base_z_off+caster_frame_z_off+caster_frame_diameter/2)/2)}" rpy="0 0 0" />
</joint>
</xacro:macro>
......@@ -203,12 +206,12 @@
<link name="base_link" />
<joint name="base_joint" type="fixed">
<parent link="base_link"/>
<child link="robot_link"/>
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
<parent link="base_link" />
<child link="robot_base_link" />
<origin xyz="${base_x_off} 0.0 ${base_z_off}" rpy="0.0 0.0 0.0" />
</joint>
<link name="robot_link">
<link name="robot_base_link">
<visual>
<geometry>
<box size="${base_length} ${base_width} ${base_height}" />
......@@ -230,14 +233,14 @@
<link name="base_footprint" />
<joint name="footprint_joint" type="fixed">
<parent link="base_link" />
<parent link="robot_base_link" />
<child link="base_footprint" />
<origin xyz="0.0 0.0 ${-(wheel_radius-wheel_z_off)}" rpy="0 0 0" />
<origin xyz="0.0 0.0 ${-(wheel_radius+base_z_off)}" rpy="0 0 0" />
</joint>
<!-- Drive wheels -->
<xacro:wheel name="left_drive_wheel" x_reflect="-1" y_reflect="1" />
<xacro:wheel name="right_drive_wheel" x_reflect="-1" y_reflect="-1" />
<xacro:wheel name="left_drive_wheel" y_reflect="1" />
<xacro:wheel name="right_drive_wheel" y_reflect="-1" />
<!-- Caster wheels -->
<xacro:caster_wheel name="left_caster" y_reflect="1" />
......@@ -246,9 +249,9 @@
<!-- Backpack frame -->
<link name="frame_base_link" />
<joint name="frame_base_joint" type="fixed">
<parent link="base_link" />
<parent link="robot_base_link" />
<child link="frame_base_link" />
<origin xyz="${frame_base_x_off} 0.0 ${frame_base_z_off}" />
<origin xyz="${-(base_length/2 + frame_base_x_gap)} 0.0 ${-frame_base_z_off}" />
</joint>
<xacro:frame_horizontal name="frame_bottom" z_off="${frame_bottom_zoff}" />
......@@ -280,7 +283,7 @@
<joint name="caster_frame_parallel_joint" type="fixed">
<origin xyz="${base_length/2} 0.0 ${caster_frame_z_off} " rpy="0.0 0.0 0.0" />
<parent link="base_link" />
<parent link="robot_base_link" />
<child link="caster_frame_parallel_link" />
</joint>
......@@ -340,6 +343,34 @@
</xacro:sensor_d435i>
<!-- * GAZEBO * -->
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace>/demo</namespace>
</ros>
<!-- wheels -->
<left_joint>left_drive_wheel_joint</left_joint>
<right_joint>right_drive_wheel_joint</right_joint>
<!-- kinematics -->
<wheel_separation>${base_width + 2*wheel_y_gap}</wheel_separation>
<wheel_diameter>${2*wheel_radius}</wheel_diameter>
<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
</plugin>
</gazebo>
<gazebo reference="${back_camera_name}_gyro_frame">
<sensor name="imu_sensor" type="imu">
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
......@@ -408,32 +439,4 @@
</imu>
</sensor>
</gazebo>
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace>/demo</namespace>
</ros>
<!-- wheels -->
<left_joint>left_drive_wheel_joint</left_joint>
<right_joint>right_drive_wheel_joint</right_joint>
<!-- kinematics -->
<wheel_separation>${base_width + 2*wheel_y_gap}</wheel_separation>
<wheel_diameter>${2*wheel_radius}</wheel_diameter>
<!-- limits -->
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>false</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
</plugin>
</gazebo>
</robot>
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment