sim with small drift
This commit is contained in:
34
src/sherpa/config/ackermann_controller.yaml
Normal file
34
src/sherpa/config/ackermann_controller.yaml
Normal file
@@ -0,0 +1,34 @@
|
||||
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100
|
||||
|
||||
ackermann_controller:
|
||||
type: ackermann_steering_controller/AckermannSteeringController
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: "joint_state_broadcaster/JointStateBroadcaster"
|
||||
|
||||
|
||||
ackermann_controller:
|
||||
ros__parameters:
|
||||
position_feedback: false
|
||||
|
||||
# Interfaces
|
||||
rear_wheels_names: ["wheel_bl_joint", "wheel_br_joint"]
|
||||
front_wheels_names: ["steer_fl_joint", "steer_fr_joint"]
|
||||
|
||||
# Physical parameters
|
||||
wheelbase: 0.6 # Distance between front and rear axles [m]
|
||||
front_wheel_track: 0.4 # Distance between front wheels [m]
|
||||
rear_wheel_track: 0.5 # Distance between rear wheels [m]
|
||||
front_wheels_radius: 0.15 # Radius of wheels [m]
|
||||
rear_wheels_radius: 0.15 # Radius of wheels [m]
|
||||
|
||||
# Limits
|
||||
max_steering_angle: 0.785 # [rad] ~45 degrees
|
||||
max_speed: 10.0 # [m/s]
|
||||
min_speed: -5.0 # [m/s]
|
||||
|
||||
# Frame config
|
||||
reference_frame: base_link
|
||||
@@ -49,7 +49,7 @@ def generate_launch_description():
|
||||
controller = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['mecanum_controller'],
|
||||
arguments=['ackermann_controller'],
|
||||
output='screen'
|
||||
)
|
||||
|
||||
@@ -89,7 +89,7 @@ def generate_launch_description():
|
||||
arguments=[
|
||||
"--ros-args",
|
||||
"-p",
|
||||
f"config_file:={os.path.join(get_package_share_directory('sherpa'), 'config', 'gz_bridge.yaml')}",
|
||||
f"config_file:={os.path.join(pkg_share, 'config', 'gz_bridge.yaml')}",
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
84
src/sherpa/urdf/components/wheel.xacro
Normal file
84
src/sherpa/urdf/components/wheel.xacro
Normal file
@@ -0,0 +1,84 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<material name="wheel_rim_material">
|
||||
<ambient>0.05 0.05 0.2 05</ambient>
|
||||
<diffuse>0.9 0.9 0.9 1</diffuse>
|
||||
<specular>0.1 0.1 0.1 1</specular>
|
||||
<emissive>0 0 0 0</emissive>
|
||||
<color rgba="0.025 0.025 0.025 1"/>
|
||||
</material>
|
||||
|
||||
<material name="wheel_spoke_material">
|
||||
<ambient>0.2 0.2 0.2 1</ambient>
|
||||
<diffuse>0.25 0.25 0.25 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<emissive>0 0 0 0 1</emissive>
|
||||
<color rgba="0.8 0.0 0.0 1"/>
|
||||
</material>
|
||||
|
||||
<xacro:macro name="wheel" params="name parent xyz rpy radius mass">
|
||||
|
||||
<xacro:property name="mesh_scale" value="${0.0078 * radius}"/>
|
||||
|
||||
<link name='${name}_link'>
|
||||
<inertial>
|
||||
<mass value="${mass}"/>
|
||||
<inertia
|
||||
ixx="${(13.0/48.0) * mass * radius * radius}"
|
||||
ixy="0.0"
|
||||
ixz="0.0"
|
||||
iyy="${0.5 * mass * radius * radius}"
|
||||
iyz="0.0"
|
||||
izz="${(13.0/48.0) * mass * radius * radius}"
|
||||
/>
|
||||
</inertial>
|
||||
<visual name='visual'>
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://sherpa/urdf/meshes/wheel_cape.stl"
|
||||
scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
|
||||
/>
|
||||
</geometry>
|
||||
<material name="wheel_rim_material"/>
|
||||
</visual>
|
||||
<visual name='visual'>
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="package://sherpa/urdf/meshes/wheel_spoke.stl"
|
||||
scale="${mesh_scale} ${mesh_scale} ${mesh_scale}"
|
||||
/>
|
||||
</geometry>
|
||||
<material name="wheel_spoke_material"/>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<origin xyz="0 0 0" rpy="-1.5707 0 0"/>
|
||||
<geometry>
|
||||
<cylinder radius="${radius}" length="${radius/1.5}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<xacro:element xacro:name="gazebo" reference="${name}_link">
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1.5</mu>
|
||||
<mu2>1.2</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<soft_cfm>0.01</soft_cfm> <!-- Soft contact to prevent rigid collisions -->
|
||||
<soft_erp>0.1</soft_erp> <!-- Error reduction parameter (makes contact softer) -->
|
||||
</surface>
|
||||
</xacro:element>
|
||||
<joint name='${name}_joint' type='continuous'>
|
||||
<parent link="${parent}_link"/>
|
||||
<child link="${name}_link"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit
|
||||
velocity="35"
|
||||
effort="350"
|
||||
/>
|
||||
<origin xyz="${xyz}" rpy="${rpy}"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
BIN
src/sherpa/urdf/meshes/wheel_cape.stl
Normal file
BIN
src/sherpa/urdf/meshes/wheel_cape.stl
Normal file
Binary file not shown.
BIN
src/sherpa/urdf/meshes/wheel_spoke.stl
Normal file
BIN
src/sherpa/urdf/meshes/wheel_spoke.stl
Normal file
Binary file not shown.
@@ -2,7 +2,7 @@
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">
|
||||
<gazebo>
|
||||
<plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system.so">
|
||||
<parameters>$(find sherpa)/config/mecanum_controller.yaml</parameters>
|
||||
<parameters>$(find sherpa)/config/ackermann_controller.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
@@ -21,34 +21,66 @@
|
||||
</link>
|
||||
|
||||
<xacro:include filename="components/imu.xacro"/>
|
||||
<xacro:include filename="components/mecanum_wheel.xacro"/>
|
||||
<xacro:include filename="components/wheel.xacro"/>
|
||||
<xacro:include filename="components/ouster_lidar.xacro"/>
|
||||
|
||||
<xacro:imu name="imu" parent="base" xyz="0 0 0.156" rpy="0 0 0" frequency="300"/>
|
||||
<xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.15" rpy="0 0 0" model="vlp16" frequency="10" topic="/points"/>
|
||||
|
||||
<xacro:mecanum_wheel name="wheel_fr" parent="base" xyz=" 0.31 -0.31 0" rpy="0 0 0" direction="r" radius="0.1" mass="10"/>
|
||||
<xacro:mecanum_wheel name="wheel_fl" parent="base" xyz=" 0.31 0.31 0" rpy="0 0 0" direction="l" radius="0.1" mass="10"/>
|
||||
<xacro:mecanum_wheel name="wheel_br" parent="base" xyz="-0.31 -0.31 0" rpy="0 0 0" direction="l" radius="0.1" mass="10"/>
|
||||
<xacro:mecanum_wheel name="wheel_bl" parent="base" xyz="-0.31 0.31 0" rpy="0 0 0" direction="r" radius="0.1" mass="10"/>
|
||||
<link name="steer_fl_link">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia
|
||||
ixx="0.0"
|
||||
ixy="0.0"
|
||||
ixz="0.0"
|
||||
iyy="0.0"
|
||||
iyz="0.0"
|
||||
izz="0.0"
|
||||
/>
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="steer_fr_link">
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia
|
||||
ixx="0.0"
|
||||
ixy="0.0"
|
||||
ixz="0.0"
|
||||
iyy="0.0"
|
||||
iyz="0.0"
|
||||
izz="0.0"
|
||||
/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<xacro:wheel name="wheel_fl" parent="steer_fl" xyz="0 0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
|
||||
<xacro:wheel name="wheel_fr" parent="steer_fr" xyz="0 -0.05 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
|
||||
<xacro:wheel name="wheel_bl" parent="base" xyz="-0.3 0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
|
||||
<xacro:wheel name="wheel_br" parent="base" xyz="-0.3 -0.25 0" rpy="0 0 0" radius="0.15" mass="2.5"/>
|
||||
|
||||
<joint name="steer_fl_joint" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="steer_fl_link"/>
|
||||
<origin xyz="0.3 0.20 0" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
|
||||
</joint>
|
||||
|
||||
<joint name="steer_fr_joint" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="steer_fr_link"/>
|
||||
<origin xyz="0.3 -0.20 0" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit upper="0.3" lower="-0.3" effort="-1" velocity="-1"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<ros2_control name="SherpaSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="wheel_fl_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
|
||||
<joint name="wheel_fr_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
|
||||
<joint name="wheel_bl_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="velocity"/>
|
||||
@@ -60,5 +92,15 @@
|
||||
<state_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
|
||||
<joint name="steer_fl_joint">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
|
||||
<joint name="steer_fr_joint">
|
||||
<command_interface name="position"/>
|
||||
<state_interface name="position"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
</robot>
|
||||
@@ -36,7 +36,7 @@
|
||||
<linear>0.05</linear>
|
||||
<quadratic>0.01</quadratic>
|
||||
</attenuation>
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<cast_shadows>false</cast_shadows>
|
||||
</light>
|
||||
|
||||
<model name="ground_plane">
|
||||
|
||||
Reference in New Issue
Block a user