mobile articulated robot wheels slipping instead of rolling
Hi there everyone and thanks for helping. A quick bit of background, I've built an articulated center steering four(4) wheeled mobile robot. I'm struggling to find any navigation packages which suit this type of geometry (currently using TEB nav, badly) so thought I would try RL (reinforcement learning) using robogym but firstly I need my robot to work properly in the simulator.
This brings me to my problem, when I articulate in the sim, the front wheels just slide sideways. I have an animated GIF of the real robot and one of the sim robot (which I cant add to this post because I don't have any karma)
After you launch all the nodes and everything is running if you publish a command to the controller manager to rotate the center steering you will see the front section rotate which will slide the wheels (no roller or rotation). I reality it should bend in the middle and all wheels should turn slightly.
Why are my wheels sliding? I have tried heaps of different 'mu' values, I've tried conemodel friction, I've tried bullet physics, I've tried the fdir option, adjusting the contactsurface_layer distance, nothing seems to make any difference. Any help is really appreciated, thanks in advance..
Run this command to articulate:
$ rostopic pub /artmule_body/fpj_position_controller/command std_msgs/Float64 "data: -0.50"
This is my world launch file /artmule_body/launch/gazebo.launch:
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="extra_gazebo_args" default=""/>
<arg name="gui" default="true"/>
<arg name="recording" default="false"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="physics" default="ode"/>
<arg name="verbose" default="false"/>
<arg name="output" default="screen"/>
<arg name="world_name" default="$(find artmule_body)/world/empty_world.world"/>
<arg name="respawn_gazebo" default="false"/>
<arg name="use_clock_frequency" default="false"/>
<arg name="pub_clock_frequency" default="100"/>
<arg name="enable_ros_network" default="true" />
<arg name="server_required" default="false"/>
<arg name="gui_required" default="false"/>
<!-- set use_sim_time flag -->
<param name="/use_sim_time" value="$(arg use_sim_time)"/>
<!-- set command arguments -->
<arg unless="$(arg paused)" name="command_arg1" value=""/>
<arg if="$(arg paused)" name="command_arg1" value="-u"/>
<arg unless="$(arg recording)" name="command_arg2" value=""/>
<arg if="$(arg recording)" name="command_arg2" value="-r"/>
<arg unless="$(arg verbose)" name="command_arg3" value=""/>
<arg if="$(arg verbose)" name="command_arg3" value="--verbose"/>
<arg unless="$(arg debug)" name="script_type" value="gzserver"/>
<arg if="$(arg debug)" name="script_type" value="debug"/>
<!-- start gazebo server-->
<group if="$(arg use_clock_frequency)">
<param name="gazebo/pub_clock_frequency" value="$(arg pub_clock_frequency)" />
</group>
<group>
<param name="gazebo/enable_ros_network" value="$(arg enable_ros_network)" />
</group>
<node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="$(arg respawn_gazebo)" output="$(arg output)"
args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)"
required="$(arg server_required)" />
<!-- start gazebo client -->
<group if="$(arg gui)">
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="$(arg output)" args="$(arg command_arg3)"
required="$(arg gui_required)"/>
</group>
<node
name="tf_footprint_base"
pkg="tf"
type="static_transform_publisher"
args="0 0 0 0 0 0 base_link base_footprint 40" />
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description" command="cat $(find artmule_body)/urdf/artmule_body.urdf" />
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model artmule_body -param robot_description"/>
</launch>
This is the empty world file /artmulebody/world/emptyworld.world:
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<ode>
<solver>
<type>quick</type>
<iters>51</iters>
<sor>1.3</sor>
<friction_model>pyramid_model</friction_model>
</solver>
<constraints>
<cfm>0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
</physics>
</world>
</sdf>
The robot URDF file /artmulebody/urdf/artmulebody.urdf (note: this file was mostly generated by Solidworks, but I've added gazebo tags and some ode parameters and so on. The meshes are large and seem to display properly in gazebo (frame and wheels visuals). I adjusted the
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
Commit Version: 1.5.1-0-g916b5db Build Version: 1.5.7152.31018
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot name="artmule_body" xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<!--plugin name="gazebo_ros_wheel_slip" filename="libgazebo_ros_wheel_slip.so"-->
<robotNamespace>/artmule_body</robotNamespace>
<!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
</plugin>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="Lidar_Link">
<material>Gazebo/White</material>
</gazebo>
<gazebo reference="Front_Link">
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="Front_Left_Axle">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="Front_Right_Axle">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="Rear_Left_Axle">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="Rear_Right_Axle_Link">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="Front_Left_Wheel_Link">
<material>Gazebo/Red</material>
<collision>
</collision>
</gazebo>
<gazebo reference="Front_Right_Wheel_Link">
<material>Gazebo/FlatBlack</material>
<collision>
</collision>
</gazebo>
<gazebo reference="Rear_Left_Wheel_Link">
<material>Gazebo/FlatBlack</material>
<collision>
</collision>
</gazebo>
<gazebo reference="Rear_Right_Wheel_Link">
<material>Gazebo/FlatBlack</material>
<collision>
</collision>
</gazebo>
<pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
<link name="base_link">
<inertial>
<origin
xyz="0.0 0.0 0.0"
rpy="0 0 0" />
<mass
value="3" />
<inertia
ixx="0.001"
ixy="0"
ixz="0"
iyy="0.001"
iyz="0"
izz="0.001" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/base_link.STL" />
</geometry>
</collision>
</link>
<link name="Lidar_Link">
<inertial>
<origin
xyz="0.0158797109484339 0.029853788807204 0.022237461889554"
rpy="0 0 0" />
<mass
value="0.0772994590865252" />
<inertia
ixx="6.64800833992454E-05"
ixy="-1.73920458974712E-06"
ixz="-3.32295669660391E-06"
iyy="5.81836937338085E-05"
iyz="-5.34492302815605E-07"
izz="3.62703552458512E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Lidar_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Lidar_Link.STL" />
</geometry>
</collision>
</link>
<joint name="Lidar_Joint" type="continuous">
<origin
xyz="0 0 0.42396"
rpy="1.5708 0 3.1416" />
<parent
link="base_link" />
<child
link="Lidar_Link" />
<axis
xyz="0 1 0" />
<dynamics friction="0.1"/>
</joint>
<link name="Lidar_Tilt_Link">
<inertial>
<origin
xyz="8.07638090769945E-06 -0.00100548513406157 0.0116854584766925"
rpy="0 0 0" />
<mass
value="0.046768032031124" />
<inertia
ixx="9.75790690949105E-06"
ixy="2.61084698456655E-07"
ixz="5.15965846181058E-08"
iyy="2.10140566671905E-05"
iyz="-1.09791527970008E-07"
izz="1.80470043747733E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Lidar_Tilt_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Lidar_Tilt_Link.STL" />
</geometry>
</collision>
</link>
<joint name="Lidar_Tilt_Joint" type="continuous">
<origin
xyz="-0.002 0.040877 0"
rpy="3.1416 -1.5708 0" />
<parent
link="Lidar_Link" />
<child
link="Lidar_Tilt_Link" />
<axis
xyz="1 0 0" />
<dynamics friction="1"/>
</joint>
<link name="Front_Pivot_Link">
<inertial>
<origin
xyz="-0.0237546791316809 4.74793554886817E-14 0.0694827387015425"
rpy="0 0 0" />
<mass
value="0.526573475079648" />
<inertia
ixx="0.00081861339654528"
ixy="3.0942087690996E-16"
ixz="7.29485814683638E-16"
iyy="0.00042749042229081"
iyz="4.61802877737082E-09"
izz="0.000409691525807351" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Pivot_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Pivot_Link.STL" />
</geometry>
</collision>
</link>
<joint name="Front_Pivot_Joint" type="revolute">
<origin
xyz="0 0 0.191"
rpy="0 0 -3.1416" />
<parent
link="base_link" />
<child
link="Front_Pivot_Link" />
<axis
xyz="0 0 1" />
<dynamics friction="0.1"/>
<limit lower="-0.50" upper="0.50" effort="0.0" velocity="0.1"/>
</joint>
<link name="Front_Link">
<inertial>
<origin
xyz="0.032210458084327 -2.44675140332228E-16 -0.00227904267394563"
rpy="0 0 0" />
<mass
value="1.09953495959677" />
<inertia
ixx="0.00579621374440153"
ixy="4.70295163512877E-18"
ixz="0.000865002667992356"
iyy="0.00275750881831944"
iyz="7.28186718566E-18"
izz="0.00580661102778322" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.650980392156863 0.619607843137255 0.588235294117647 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Link.STL" />
</geometry>
</collision>
</link>
<joint name="Front_Body_Joint" type="revolute">
<origin
xyz="-0.0299999999999996 0 0.0700000000000014"
rpy="0 0 3.14159265358979" />
<parent
link="Front_Pivot_Link" />
<child
link="Front_Link" />
<axis
xyz="1 0 0" />
<dynamics friction="0.1"/>
<limit lower="-0.1" upper="0.1" effort="0.0" velocity="0.01"/>
</joint>
<link name="Front_Left_Axle">
<inertial>
<origin
xyz="3.88578058618805E-16 0.0272779029779359 0"
rpy="0 0 0" />
<mass
value="0.0176819340805515" />
<inertia
ixx="2.26912683011394E-05"
ixy="-1.76941275442854E-20"
ixz="1.40141988769782E-35"
iyy="5.18755491561235E-07"
iyz="1.84609260716542E-20"
izz="2.26912683011394E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Left_Axle.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Left_Axle.STL" />
</geometry>
</collision>
</link>
<joint name="Front_Left_Axle_Joint" type="fixed">
<origin
xyz="0.231999999999999 0.175000000000008 -0.0969999999999983"
rpy="3.14159265358979 0 3.14159265358979" />
<parent
link="Front_Link" />
<child
link="Front_Left_Axle" />
<axis
xyz="0 1 0" />
</joint>
<!--
#####################################################
Front left wheel LINK <<<<
-->
<link name="Front_Left_Wheel_Link">
<inertial>
<origin
xyz="-0.0626072014447143 3.33066907387547E-16 -8.32667268468867E-17"
rpy="0 0 0" />
<mass
value="3.92074665682532" />
<inertia
ixx="0.0592279373225076"
ixy="-2.09783426676889E-17"
ixz="2.63584046080219E-17"
iyy="0.0316195974277248"
iyz="-2.5321274476343E-17"
izz="0.0316195974277248" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Left_Wheel_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
</material>
</visual>
<collision>
<origin
xyz="-0.06 0 0"
rpy="-1.570810 0 -1.570810" />
<geometry>
<!--mesh
filename="package://artmule_body/meshes/Front_Left_Wheel_Link.STL" /-->
<cylinder radius="0.162656352" length="0.06"/>
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
<fdir1>1 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
</surface>
</collision>
</link>
<joint name="Front_Left_Wheel_Joint" type="continuous">
<origin
xyz="0 0 0"
rpy="0 0 -1.5707963267949" />
<parent
link="Front_Left_Axle" />
<child
link="Front_Left_Wheel_Link" />
<axis
xyz="1 0 0" />
<limit lower="-3.1419" upper="3.1419" effort="0.0" velocity="0.0"/>
</joint>
<link name="Front_Right_Axle">
<inertial>
<origin
xyz="3.33066907387547E-16 0.0652779029779602 -1.74860126378462E-15"
rpy="0 0 0" />
<mass
value="0.0176819340805515" />
<inertia
ixx="2.26912683011394E-05"
ixy="-9.17944082682159E-23"
ixz="3.24337094634816E-38"
iyy="5.18755491561235E-07"
iyz="1.83930237390794E-20"
izz="2.26912683011394E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Right_Axle.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Right_Axle.STL" />
</geometry>
</collision>
</link>
<joint name="Front_Right_Axle_Joint" type="fixed">
<origin
xyz="0.231999999999999 -0.136999999999987 -0.0969999999999985"
rpy="0 0 3.14159265358979" />
<parent
link="Front_Link" />
<child
link="Front_Right_Axle" />
<axis
xyz="0 1 0" />
</joint>
<!--
#####################################################
Front Right wheel LINK >>>>
-->
<link name="Front_Right_Wheel_Link">
<inertial>
<origin
xyz="-0.0506072014447373 -5.55111512312578E-16 1.13797860024079E-15"
rpy="0 0 0" />
<mass
value="3.92074665682533" />
<inertia
ixx="0.0592279373225076"
ixy="1.71725648798771E-17"
ixz="1.9354269196565E-16"
iyy="0.0316195974277248"
iyz="-1.74508722187149E-17"
izz="0.0316195974277248" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Front_Right_Wheel_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
</material>
</visual>
<collision>
<origin
xyz="-0.06 0 0"
rpy="-1.570810 0 -1.570810" />
<geometry>
<!--mesh
filename="package://artmule_body/meshes/Front_Right_Wheel_Link.STL" /-->
<cylinder radius="0.162656352" length="0.06"/>
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
<fdir1>1 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
</surface>
</collision>
</link>
<joint name="Front_Right_Wheel_Joint" type="continuous">
<origin
xyz="0 0.05 0"
rpy="3.14159265358979 0 -1.5707963267949" />
<parent
link="Front_Right_Axle" />
<child
link="Front_Right_Wheel_Link" />
<axis
xyz="1 0 0" />
<limit lower="-3.1419" upper="3.1419" effort="0.0" velocity="0.0"/>
</joint>
<link name="Rear_Left_Axle">
<inertial>
<origin
xyz="5.3089199703038E-12 0.065277902977945 -1.97064586870965E-15"
rpy="0 0 0" />
<mass
value="0.0176819340805515" />
<inertia
ixx="2.26912683011394E-05"
ixy="-1.80344567611677E-15"
ixz="-1.5979906930206E-30"
iyy="5.18755491561233E-07"
iyz="-1.9675556218577E-20"
izz="2.26912683011394E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Rear_Left_Axle.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Rear_Left_Axle.STL" />
</geometry>
</collision>
</link>
<joint name="Rear_Left_Axle_Joint" type="fixed">
<origin
xyz="-0.261 0.1495 0.164"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="Rear_Left_Axle" />
<axis
xyz="0 1 0" />
</joint>
<!--
#####################################################
Rear Left wheel LINK <<<<
-->
<link name="Rear_Left_Wheel_Link">
<inertial>
<origin
xyz="-0.0506072014447241 -4.11615186379777E-12 1.11022302462516E-16"
rpy="0 0 0" />
<mass
value="3.92074665682532" />
<inertia
ixx="0.0592279373225076"
ixy="2.24556942797003E-12"
ixz="-4.33805306771177E-17"
iyy="0.0316195974277248"
iyz="-7.77034648905791E-18"
izz="0.0316195974277248" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Rear_Left_Wheel_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
</material>
</visual>
<collision>
<origin
xyz="-0.06 0 0"
rpy="-1.570810 0 -1.570810" />
<geometry>
<!--mesh
filename="package://artmule_body/meshes/Rear_Left_Wheel_Link.STL" /-->
<cylinder radius="0.162656352" length="0.06"/>
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
<fdir1>1 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
</surface>
</collision>
</link>
<joint name="Rear_Left_Wheel_Joint" type="continuous">
<origin
xyz="0 0.05 0"
rpy="3.14159265358979 0 -1.5707963267949" />
<parent
link="Rear_Left_Axle" />
<child
link="Rear_Left_Wheel_Link" />
<axis
xyz="1 0 0" />
<limit lower="-3.1419" upper="3.1419" effort="0.0" velocity="0.0"/>
</joint>
<link name="Rear_Right_Axle_Link">
<inertial>
<origin
xyz="5.30947508181612E-12 0.0652779029779457 -2.77555756156289E-17"
rpy="0 0 0" />
<mass
value="0.0176819340805515" />
<inertia
ixx="2.26912683011394E-05"
ixy="-1.80344567611677E-15"
ixz="5.74555439277144E-31"
iyy="5.18755491561234E-07"
iyz="6.96153122075381E-21"
izz="2.26912683011394E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Rear_Right_Axle_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Rear_Right_Axle_Link.STL" />
</geometry>
</collision>
</link>
<joint name="Rear_Right_Axle_Joint" type="fixed">
<origin
xyz="-0.26100000001216 -0.14949999997877 0.164000000000002"
rpy="0 0 3.14159265358979" />
<parent
link="base_link" />
<child
link="Rear_Right_Axle_Link" />
<axis
xyz="0 1 0" />
</joint>
<!--
#####################################################
Rear Right wheel LINK >>>>
-->
<link name="Rear_Right_Wheel_Link">
<inertial>
<origin
xyz="-0.0506072014447241 -4.116207374949E-12 5.55111512312578E-17"
rpy="0 0 0" />
<mass
value="3.92074665682533" />
<inertia
ixx="0.0592279373225076"
ixy="2.24557145887076E-12"
ixz="-5.95978435637949E-18"
iyy="0.0316195974277248"
iyz="-4.63148764679093E-18"
izz="0.0316195974277248" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://artmule_body/meshes/Rear_Right_Wheel_Link.STL" />
</geometry>
<material
name="">
<color
rgba="0.298039215686275 0.298039215686275 0.298039215686275 1" />
</material>
</visual>
<collision>
<origin
xyz="-0.06 0 0"
rpy="-1.570810 0 -1.570810" />
<geometry>
<!--mesh
filename="package://artmule_body/meshes/Rear_Right_Wheel_Link.STL" /-->
<cylinder radius="0.162656352" length="0.06"/>
</geometry>
<surface>
<friction>
<ode>
<mu>0.01</mu>
<mu2>0.01</mu2>
<fdir1>1 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
</friction>
</surface>
</collision>
</link>
<joint name="Rear_Right_Wheel_Joint" type="continuous">
<origin
xyz="0 0.05 0"
rpy="3.14159265358979 0 -1.5707963267949" />
<parent
link="Rear_Right_Axle_Link" />
<child
link="Rear_Right_Wheel_Link" />
<axis
xyz="1 0 0" />
<limit lower="-3.1419" upper="3.1419" effort="0.0" velocity="0.0"/>
</joint>
<transmission name="lidar_tilt_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Lidar_Tilt_Joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="lidar_tilt">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="lidar_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Lidar_Joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="lidar_rotate">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="centre_steering_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Front_Pivot_Joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="steering_actuator">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="front_left_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="Front_Left_Wheel_Joint">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="wheel_actuator">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</robot>
I have a robot control node using controller manager /artmulecontrol/launch/artmulecontrol.launch:
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find artmule_control)/config/artmule_control.yaml" command="load"/>
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/artmule_body" args="lidar_position_controller lidar_tilt_position_controller fpj_position_controller joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/artmule_body/joint_states" />
</node>
</launch>
And its yaml config file /artmulecontrol/config/artmulecontrol.yaml:
artmule_body:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
lidar_position_controller:
type: position_controllers/JointPositionController
joint: Lidar_Joint
pid: {p: 5.0, i: 0.01, d: 0.1}
lidar_tilt_position_controller:
type: position_controllers/JointPositionController
joint: Lidar_Tilt_Joint
pid: {p: 5.0, i: 0.01, d: 0.1}
fpj_position_controller:
type: position_controllers/JointPositionController
joint: Front_Pivot_Joint
pid: {p: 5.01, i: 0.001, d: 0.001}
# Velocity Controllers
flw_velocity_controller:
type: effort_controllers/JointVelocityController
joint: Front_Left_Wheel_Joint
pid: {p: 5.01, i: 0.001, d: 0.001}
Asked by zonared on 2023-04-22 02:00:37 UTC
Comments