API tools faq
paste
Advertisement
marcusvini178

truck_xacro_ackermann

marcusvini178
Mar 28th, 2023
495
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 13.58 KB | Source Code | 0 0
raw download clone embed print report
  1. <?xml version="1.0"?>
  2. <robot name="truck" xmlns:xacro="http://ros.org/wiki/xacro">
  3.  
  4.   <!-- Define truck constants -->
  5.   <xacro:property name="chassis_length" value="8.2"/>
  6.   <xacro:property name="chassis_width" value="2.505"/>
  7.   <xacro:property name="chassis_height" value="0.2"/>
  8.   <xacro:property name="wheel_radius" value="0.45"/>
  9.   <xacro:property name="wheel_width" value="0.3"/>
  10.   <xacro:property name="wheel_zoff" value="0.05"/>
  11.  
  12.   <xacro:property name="front_shaft_distance" value="2.0"/>
  13.   <xacro:property name="rear_shaft_distance" value="1.45"/>
  14.   <xacro:property name="wheelbase" value="2.5"/>
  15.  
  16.  
  17.    <!-- Define cabin and bucket properties -->
  18.   <xacro:property name="cabin_length" value="2.0"/>
  19.   <xacro:property name="cabin_width" value="2.505"/>
  20.   <xacro:property name="cabin_height" value="2.0"/>
  21.   <xacro:property name="cabin_x_offset" value="-3.15"/>
  22.  
  23.   <xacro:property name="bucket_length" value="5.5"/>
  24.   <xacro:property name="bucket_width" value="2.505"/>
  25.   <xacro:property name="bucket_height" value="1.0"/>
  26.   <xacro:property name="bucket_x_offset" value="1.25"/>
  27.  
  28.   <!-- Define axles properties for the frontal wheels-->
  29.   <xacro:property name="axle_length" value="0.4"/>
  30.   <xacro:property name="axle_radius" value="0.1"/>
  31.  
  32.   <!-- Define the frontal shaft bar size-->
  33.   <xacro:property name="steering_bar_radius" value="0.1"/>
  34.   <xacro:property name="steering_bar_length" value="2.0"/>
  35.  
  36.     <!-- Define inertial property macros -->
  37.   <xacro:macro name="box_inertia" params="m w h d">
  38.     <inertial>
  39.       <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
  40.       <mass value="${m}"/>
  41.       <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
  42.     </inertial>
  43.   </xacro:macro>
  44.  
  45.   <xacro:macro name="cylinder_inertia" params="m r h">
  46.     <inertial>
  47.      <origin xyz="0 0 0" rpy="0 0 0"/>
  48.      <mass value="${m}"/>
  49.      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (3*r*r + h*h)}" iyz="0.0" izz="${(m/2) * r*r}"/>
  50.     </inertial>
  51.   </xacro:macro>
  52.  
  53.     <!-- Bar Definition -->
  54.   <xacro:macro name="horizontal_shaft" params="xpos ypos">
  55.     <link name="horizontal_shaft_link">
  56.       <visual>
  57.         <geometry>
  58.           <cylinder radius="${steering_bar_radius}" length="${steering_bar_length}"/>
  59.         </geometry>
  60.         <material name="Black">
  61.           <color rgba="0 0 0 1.0"/>
  62.         </material>
  63.         <origin xyz="0 0 0" rpy="0 ${pi/2} 0"/>
  64.       </visual>
  65.       <collision>
  66.         <geometry>
  67.           <cylinder radius="${steering_bar_radius}" length="${steering_bar_length}"/>
  68.         </geometry>
  69.         <origin xyz="0 0 0" rpy="0 ${pi/2} 0"/>
  70.       </collision>
  71.       <xacro:cylinder_inertia m="1" r="${steering_bar_radius}" h="${steering_bar_length}"/>
  72.     </link>
  73.   </xacro:macro>
  74.  
  75.  
  76.   <!-- Horizontal shaft instance -->
  77.   <xacro:horizontal_shaft xpos="${-wheelbase / 2 - front_shaft_distance}" ypos="0"/>
  78.  
  79.   <!-- Horizontal shaft to base link connection -->
  80.   <joint name="horizontal_shaft_to_base" type="revolute">
  81.     <parent link="base_link"/>
  82.     <child link="horizontal_shaft_link"/>
  83.     <origin xyz="${-wheelbase / 2 - front_shaft_distance} 0 0" rpy="0 0 ${pi/2}"/>
  84.     <axis xyz="0 0 1"/>
  85.     <limit lower="-0.131" upper="0.131" effort="10000000" velocity="1000000"/>
  86.   </joint>
  87.  
  88.  
  89.     <!-- Define axles property macros -->
  90.   <xacro:macro name="axle" params="prefix xpos ypos">
  91.     <link name="${prefix}_axle_link">
  92.       <visual>
  93.         <geometry>
  94.           <cylinder radius="${axle_radius}" length="${axle_length}"/>
  95.         </geometry>
  96.         <material name="Black">
  97.           <color rgba="0 0 0 1.0"/>
  98.         </material>
  99.       </visual>
  100.       <collision>
  101.         <geometry>
  102.           <cylinder radius="${axle_radius}" length="${axle_length}"/>
  103.         </geometry>
  104.       </collision>
  105.       <xacro:cylinder_inertia m="1" r="${axle_radius}" h="${axle_length}"/>
  106.     </link>
  107.   </xacro:macro>
  108.  
  109.   <!-- Define Steer Joint Macros-->
  110.   <xacro:macro name="steer_joint" params="prefix xpos ypos">
  111.     <joint name="${prefix}_steer_joint" type="revolute">
  112.       <parent link="horizontal_shaft_link" />
  113.       <child link="${prefix}_axle_link" />
  114.       <origin xyz="${xpos} ${ypos} ${-0.0001}" rpy="0 0 0" />
  115.       <axis xyz="0 0 1" />
  116.       <limit lower="-0.524" upper="0.524" effort="10000000" velocity="1000000" />
  117.     </joint>
  118.   </xacro:macro>
  119.  
  120.  
  121.     <!-- Chassis -->
  122.   <link name="base_link">
  123.     <visual>
  124.       <geometry>
  125.         <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
  126.       </geometry>
  127.       <material name="Blue">
  128.         <color rgba="0 0 1.0 1.0"/>
  129.       </material>
  130.     </visual>
  131.     <collision>
  132.       <geometry>
  133.         <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
  134.       </geometry>
  135.     </collision>
  136.  
  137.     <xacro:box_inertia m="500" w="${chassis_width}" d="${chassis_length}" h="${chassis_height}"/>
  138.   </link>
  139.  
  140.   <!-- Robot Footprint -->
  141.   <link name="base_footprint"/>
  142.  
  143.   <joint name="base_joint" type="fixed">
  144.     <parent link="base_link"/>
  145.     <child link="base_footprint"/>
  146.     <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
  147.   </joint>
  148.  
  149.   <!-- Cabin -->
  150.   <link name="cabin_link">
  151.     <visual>
  152.       <geometry>
  153.         <box size="${cabin_length} ${cabin_width} ${cabin_height}"/>
  154.       </geometry>
  155.       <material name="Green">
  156.         <color rgba="0 1.0 0 1.0"/>
  157.       </material>
  158.     </visual>
  159.     <collision>
  160.       <geometry>
  161.         <box size="${cabin_length} ${cabin_width} ${cabin_height}"/>
  162.       </geometry>
  163.     </collision>
  164.  
  165.     <xacro:box_inertia m="300" w="${cabin_width}" d="${cabin_length}" h="${cabin_height}"/>
  166.   </link>
  167.  
  168.   <joint name="cabin_joint" type="fixed">
  169.     <parent link="base_link"/>
  170.     <child link="cabin_link"/>
  171.     <origin xyz="${cabin_x_offset} 0 ${chassis_height / 2 + cabin_height / 2}" rpy="0 0 0"/>
  172.   </joint>
  173.  
  174.   <!-- Bucket -->
  175.   <link name="bucket_link">
  176.     <visual>
  177.       <geometry>
  178.         <!-- <mesh filename="package://your_package_name/meshes/bucket.stl"/> -->
  179.         <box size="${bucket_length} ${bucket_width} ${bucket_height}"/>
  180.       </geometry>
  181.       <material name="Orange">
  182.         <color rgba="1.0 0.5 0 1.0"/>
  183.       </material>
  184.     </visual>
  185.     <collision>
  186.       <geometry>
  187.         <!-- <mesh filename="package://your_package_name/meshes/bucket_collision.stl"/> -->
  188.         <box size="${bucket_length} ${bucket_width} ${bucket_height}"/>
  189.       </geometry>
  190.     </collision>
  191.  
  192.     <xacro:box_inertia m="400" w="${bucket_width}" d="${bucket_length}" h="${bucket_height}"/>
  193.   </link>
  194.  
  195.   <joint name="bucket_joint" type="fixed">
  196.     <parent link="base_link"/>
  197.     <child link="bucket_link"/>
  198.     <origin xyz="${bucket_x_offset} 0 ${chassis_height / 2 + bucket_height / 2}" rpy="0 0 0"/>
  199.   </joint>
  200.  
  201.  
  202.     <!-- Wheels -->
  203.   <xacro:macro name="wheel" params="prefix xpos ypos axle_joint:='base_link' yaw:=0">
  204.     <link name="${prefix}_wheel_link">
  205.       <visual>
  206.         <origin xyz="0 0 0" rpy="${pi/2} 0 ${yaw}"/>
  207.         <geometry>
  208.           <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
  209.         </geometry>
  210.         <material name="Black">
  211.           <color rgba="0 0 0 1.0"/>
  212.         </material>
  213.       </visual>
  214.       <collision>
  215.         <origin xyz="0 0 0" rpy="${pi/2} 0 ${yaw}"/>
  216.         <geometry>
  217.           <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
  218.         </geometry>
  219.       </collision>
  220.  
  221.       <xacro:cylinder_inertia m="50" r="${wheel_radius}" h="${wheel_width}"/>
  222.     </link>
  223.  
  224.     <joint name="${prefix}_wheel_joint" type="continuous">
  225.       <parent link="${axle_joint if axle_joint is not None else 'base_link'}"/>
  226.       <child link="${prefix}_wheel_link"/>
  227.       <origin xyz="${xpos} ${ypos} ${-wheel_radius}" rpy="0 0 0"/>
  228.       <axis xyz="1 0 0"/>
  229.     </joint>
  230.   </xacro:macro>
  231.  
  232.  
  233.   <xacro:steer_joint prefix="front_right" xpos="${-chassis_width /2}" ypos="${0.0001}" />
  234.   <xacro:steer_joint prefix="front_left" xpos="${chassis_width /2}" ypos="${-0.0001}" />
  235.   <xacro:axle prefix="front_right" xpos="${-0.0001}" ypos="${0.0001}" />
  236.   <xacro:axle prefix="front_left" xpos="${-0.0001}" ypos="${-0.0001}" />
  237.   <xacro:wheel prefix="front_right" xpos="${-0.0001}" ypos="${0.0001}" axle_joint="front_left_axle_link" yaw="${pi/2}" />
  238.   <xacro:wheel prefix="front_left" xpos="${-0.0001}" ypos="${0.0001}" axle_joint="front_right_axle_link" yaw="${pi/2}" />
  239.   <xacro:wheel prefix="middle_right" xpos="${-wheelbase / 2}" ypos="${chassis_width / 2}"/>
  240.   <xacro:wheel prefix="middle_left" xpos="${-wheelbase / 2}" ypos="${-chassis_width / 2}"/>
  241.   <xacro:wheel prefix="rear_right" xpos="${wheelbase / 2}" ypos="${chassis_width / 2}"/>
  242.   <xacro:wheel prefix="rear_left" xpos="${wheelbase / 2}" ypos="${-chassis_width / 2}"/>
  243.   <xacro:wheel prefix="rear_second_right" xpos="${wheelbase / 2 + rear_shaft_distance}" ypos="${chassis_width / 2}"/>
  244.   <xacro:wheel prefix="rear_second_left" xpos="${wheelbase / 2 + rear_shaft_distance}" ypos="${-chassis_width / 2}"/>
  245.  
  246.   <link name="imu_link">
  247.     <visual>
  248.       <geometry>
  249.         <box size="0.1 0.1 0.1"/>
  250.       </geometry>
  251.     </visual>
  252.  
  253.     <collision>
  254.       <geometry>
  255.         <box size="0.1 0.1 0.1"/>
  256.       </geometry>
  257.     </collision>
  258.  
  259.     <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
  260.   </link>
  261.  
  262.   <joint name="imu_joint" type="fixed">
  263.     <parent link="base_link"/>
  264.     <child link="imu_link"/>
  265.     <origin xyz="0 0 0.01"/>
  266.   </joint>
  267.  
  268.   <gazebo reference="imu_link">
  269.     <sensor name="imu_sensor" type="imu">
  270.     <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
  271.         <ros>
  272.           <namespace>/demo</namespace>
  273.           <remapping>~/out:=imu</remapping>
  274.         </ros>
  275.         <initial_orientation_as_reference>false</initial_orientation_as_reference>
  276.       </plugin>
  277.       <always_on>true</always_on>
  278.       <update_rate>100</update_rate>
  279.       <visualize>true</visualize>
  280.       <imu>
  281.         <angular_velocity>
  282.           <x>
  283.             <noise type="gaussian">
  284.               <mean>0.0</mean>
  285.               <stddev>2e-4</stddev>
  286.               <bias_mean>0.0000075</bias_mean>
  287.               <bias_stddev>0.0000008</bias_stddev>
  288.             </noise>
  289.           </x>
  290.           <y>
  291.             <noise type="gaussian">
  292.               <mean>0.0</mean>
  293.               <stddev>2e-4</stddev>
  294.               <bias_mean>0.0000075</bias_mean>
  295.               <bias_stddev>0.0000008</bias_stddev>
  296.             </noise>
  297.           </y>
  298.           <z>
  299.             <noise type="gaussian">
  300.               <mean>0.0</mean>
  301.               <stddev>2e-4</stddev>
  302.               <bias_mean>0.0000075</bias_mean>
  303.               <bias_stddev>0.0000008</bias_stddev>
  304.             </noise>
  305.           </z>
  306.         </angular_velocity>
  307.         <linear_acceleration>
  308.           <x>
  309.             <noise type="gaussian">
  310.               <mean>0.0</mean>
  311.               <stddev>1.7e-2</stddev>
  312.               <bias_mean>0.1</bias_mean>
  313.               <bias_stddev>0.001</bias_stddev>
  314.             </noise>
  315.           </x>
  316.           <y>
  317.             <noise type="gaussian">
  318.               <mean>0.0</mean>
  319.               <stddev>1.7e-2</stddev>
  320.               <bias_mean>0.1</bias_mean>
  321.               <bias_stddev>0.001</bias_stddev>
  322.             </noise>
  323.           </y>
  324.           <z>
  325.             <noise type="gaussian">
  326.               <mean>0.0</mean>
  327.               <stddev>1.7e-2</stddev>
  328.               <bias_mean>0.1</bias_mean>
  329.               <bias_stddev>0.001</bias_stddev>
  330.             </noise>
  331.           </z>
  332.         </linear_acceleration>
  333.       </imu>
  334.     </sensor>
  335.   </gazebo>
  336.  
  337.   <plugin name="gazebo_ros_ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">
  338.     <ros>
  339.       <namespace>/demo</namespace>
  340.       <parameter name="cmd_vel_topic" value="cmd_demo"/>
  341.       <parameter name="odom_topic" value="odom_demo"/>
  342.       <parameter name="distance_topic" value="distance_demo"/>
  343.     </ros>
  344.  
  345.     <update_rate>100.0</update_rate>
  346.  
  347.     <!-- wheels -->
  348.     <front_left_joint>front_left_wheel_joint</front_left_joint>
  349.     <front_right_joint>front_right_wheel_joint</front_right_joint>
  350.     <middle_left_joint>middle_left_wheel_joint</middle_left_joint>
  351.     <middle_right_joint>middle_right_wheel_joint</middle_right_joint>
  352.     <rear_left_joint>rear_left_wheel_joint</rear_left_joint>
  353.     <rear_right_joint>rear_right_wheel_joint</rear_right_joint>
  354.     <rear_second_left_joint>rear_second_left_wheel_joint</rear_second_left_joint>
  355.     <rear_second_right_joint>rear_second_right_wheel_joint</rear_second_right_joint>
  356.  
  357.     <!-- steering joints -->
  358.     <front_left_steering_joint>front_left_steer_joint</front_left_steering_joint>
  359.     <front_right_steering_joint>front_right_steer_joint</front_right_steering_joint>
  360.  
  361.     <!-- Other parameters remain the same -->
  362.     <max_steer>0.6458</max_steer>
  363.     <max_steering_angle>7.85</max_steering_angle>
  364.     <max_speed>20</max_speed>
  365.  
  366.     <!-- PID tuning -->
  367.     <left_steering_pid_gain>1500 0 1</left_steering_pid_gain>
  368.     <left_steering_i_range>0 0</left_steering_i_range>
  369.     <right_steering_pid_gain>1500 0 1</right_steering_pid_gain>
  370.     <right_steering_i_range>0 0</right_steering_i_range>
  371.     <linear_velocity_pid_gain>1000 0 1</linear_velocity_pid_gain>
  372.     <linear_velocity_i_range>0 0</linear_velocity_i_range>
  373.  
  374.     <!-- output -->
  375.     <publish_odom>true</publish_odom>
  376.     <publish_odom_tf>true</publish_odom_tf>
  377.     <publish_wheel_tf>true</publish_wheel_tf>
  378.     <publish_distance>true</publish_distance>
  379.  
  380.     <odometry_frame>odom_demo</odometry_frame>
  381.     <robot_base_frame>chassis</robot_base_frame>
  382.   </plugin>
  383.  
  384.  
  385.  
  386. </robot>
Tags: Xacro urdf
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!