0
$\begingroup$

Rosanswers logo

Hi everyone. I created a .URDF from SolidWorks using SW2URDF plugin. This urdf file that I created, opens in rviz smoothly, however this urdf file opens in the gazebo incorrectly and the model file has missing links and joints.

moving joints have linkage and linkage joints(eg: wheel) but none for fixed joints(eg: lidar,realsense ..).

HOW CAN I SOLVE THE PROBLEM?

The first time I run the file gazebo.Launch is the model gazebo image(wrong):

image description

the image that appears when I reload the model after pressing the stop button on the lower toolbar: image description

(model reload command: rosrun gazebo_ros spawn_model -file rospack find car_design_full/urdf/car_design_full.urdf -urdf -z 0.1 -model my_object)

but the same mistakes still continue ( the model file has missing links and joints.)

Gazebo errors that occur when I edit the model file:
image description

Error [Param.hh:266] Unable to set parameter[velocity].Type type used must have a stream input and outputoperator, which allow boost::lexical_cast tofunction properly.
Error [Param.hh:266] Unable to set parameter[acceleration].Type type used must have a stream input and outputoperator, which allow boost::lexical_cast tofunction properly.
Error [Param.hh:266] Unable to set parameter[wrench].Type type used must have a stream input and outputoperator, which allow boost::lexical_cast tofunction properly.

log file: /home/ubuntu/.ros/log/952cd6c8-f367-11e7-a297-448a5bed66ea/spawn_urdf-4*.log:

[rospy.client][INFO] 2018-01-07 08:49:06,802: init_node, name[/spawn_urdf], pid[7183]
[xmlrpc][INFO] 2018-01-07 08:49:06,803: XML-RPC server binding to 0.0.0.0:0
[xmlrpc][INFO] 2018-01-07 08:49:06,803: Started XML-RPC server [http://192.168.1.38:45182/]
[rospy.init][INFO] 2018-01-07 08:49:06,803: ROS Slave URI: [http://192.168.1.38:45182/]
[rospy.impl.masterslave][INFO] 2018-01-07 08:49:06,803: _ready: http://192.168.1.38:45182/
[xmlrpc][INFO] 2018-01-07 08:49:06,804: xml rpc node: starting XML-RPC server
[rospy.registration][INFO] 2018-01-07 08:49:06,804: Registering with master node http://192.168.1.38:11311
[rospy.init][INFO] 2018-01-07 08:49:06,903: registered with master
[rospy.rosout][INFO] 2018-01-07 08:49:06,904: initializing /rosout core topic
[rospy.rosout][INFO] 2018-01-07 08:49:06,905: connected to core topic /rosout
[rospy.simtime][INFO] 2018-01-07 08:49:06,906: initializing /clock core topic
[rospy.simtime][INFO] 2018-01-07 08:49:06,907: connected to core topic /clock
[rosout][INFO] 2018-01-07 08:49:06,909: Loading model xml from file
[rosout][INFO] 2018-01-07 08:49:06,910: Waiting for service /gazebo/spawn_urdf_model
[rospy.internal][INFO] 2018-01-07 08:49:07,177: topic[/rosout] adding connection to [/rosout], count 0
[rospy.internal][INFO] 2018-01-07 08:49:08,982: topic[/clock] adding connection to [http://192.168.1.38:34827/], count 0
[rosout][INFO] 2018-01-07 08:49:09,020: Calling service /gazebo/spawn_urdf_model
[rosout][INFO] 2018-01-07 08:49:09,327: Spawn status: SpawnModel: Successfully spawned model
[rospy.core][INFO] 2018-01-07 08:49:09,327: signal_shutdown [atexit]
[rospy.internal][INFO] 2018-01-07 08:49:09,329: topic[/rosout] removing connection to /rosout
[rospy.internal][INFO] 2018-01-07 08:49:09,329: topic[/clock] removing connection to http://192.168.1.38:34827/
[rospy.impl.masterslave][INFO] 2018-01-07 08:49:09,330: atexit
[rospy.internal][WARNING] 2018-01-07 08:49:09,332: Unknown error initiating TCP/IP socket to 192.168.1.38:39439 (http://192.168.1.38:34827/): Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 558, in connect
    self.local_endpoint = self.socket.getsockname()
AttributeError: 'NoneType' object has no attribute 'getsockname

car_design_full.urdf file:

<?xml version="1.0"?>
<robot
  name="car_design_full">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0.0549311783486896 -0.00128164681408088 0.0412682875501926"
        rpy="0 0 0" />
      <mass
        value="0.555250122552161" />
      <inertia
        ixx="0.000359830381195638"
        ixy="7.5213276117718E-05"
        ixz="-0.000347705877421566"
        iyy="0.00207775632580144"
        iyz="8.91904561064106E-06"
        izz="0.00227485560466115" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.929411765 0.619607843 0.149019608 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
 <link
    name="imu">
    <inertial>
      <origin
        xyz="0.00121858913340156 0.0607046437197767 -0.00251838480456676"
        rpy="0 0 0" />
      <mass
        value="0.154902068029993" />
      <inertia
        ixx="5.39673884466214E-06"
        ixy="-1.84712467188264E-06"

        ixz="-7.34117511298518E-08"

        iyy="6.41719196851692E-07"
        iyz="-2.14148639602142E-07"
        izz="6.02143588066614E-06" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.05 0.1"/>
      </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>
       <box size="0.1 0.05 0.1"/>
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_imu"
    type="fixed">
    <origin
      xyz="-0.209 0 -0.0127500000000001"

      rpy="3.14 3.14 0" />
    <parent
      link="base_link" />
    <child
      link="imu" />
    <axis
      xyz="0 0 0" />
  </joint>

  <link
    name="h_realsense_0">
    <inertial>
      <origin
        xyz="0.00121858913340156 0.0607046437197767 -0.00251838480456676"
        rpy="0 0 0" />
      <mass
        value="0.154902068029993" />
      <inertia
        ixx="5.39673884466214E-06"
        ixy="-1.84712467188264E-06"
        ixz="-7.34117511298518E-08"
        iyy="6.41719196851692E-07"
        iyz="-2.14148639602142E-07"
        izz="6.02143588066614E-06" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_realsense_0.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://car_design_full/meshes/h_realsense_0.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_realsense_0"
    type="fixed">
    <origin
      xyz="-0.209 0 -0.0127500000000001"
      rpy="1.56712892198568 -2.13282722758847E-15 -1.57079632681354" />
    <parent
      link="base_link" />
    <child
      link="h_realsense_0" />
    <axis
      xyz="1 0 0" />
  </joint>
  <link
    name="laser">
    <inertial>
      <origin
        xyz="-0.00012001557604524 -0.178880591593867 0.139853389850626"
        rpy="0 0 0" />
      <mass
        value="7.15265038550835" />
      <inertia
        ixx="0.117894203905473"
        ixy="4.46969721048415E-05"
        ixz="6.88530346256059E-06"
        iyy="0.186161143060294"
        iyz="-9.03111997830799E-05"
        izz="0.0882812439314936" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="1.57 0 -1.57" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/s_lms111.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.541176471 0.541176471 0.541176471 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/s_lms111.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_lms11_0"
    type="fixed">
    <origin
      xyz="-0.140999999999995 0 0.150724325996435"
      rpy="3.14 3.14 0" />
    <parent
      link="base_link" />
    <child
      link="laser" />
    <axis
      xyz="0 0 0" />
    <limit effort="100" velocity="1.178465545" lower="0.0" upper="3.14" />
  </joint>
  <link
    name="h_right_wheel_0">
    <inertial>
      <origin
        xyz="5.38633027069579E-11 2.27216856441004E-12 0.0306185726073418"
        rpy="0 0 0" />
      <mass
        value="0.46815549529416" />
      <inertia
        ixx="9.06243066733727E-05"
        ixy="7.6762360844921E-22"
        ixz="9.06265002363335E-13"
        iyy="9.06243066733726E-05"
        iyz="-5.07332126035151E-15"
        izz="9.45652676048254E-21" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_right_wheel_0.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_right_wheel_0.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_wheel_right_0"
    type="continuous">
    <origin
      xyz="-0.146 -0.08296737166363 -0.0346340445573322"
      rpy="1.66119046247108 0.263481644995697 0.0236025247462862" />
    <parent
      link="base_link" />
    <child
      link="h_right_wheel_0" />
    <axis
      xyz="0 0 1" />
  </joint>
  <link
    name="h_right_wheel_2">
    <inertial>
      <origin
        xyz="-5.17153195267461E-11 -1.97974692195402E-11 0.0306180371284931"
        rpy="0 0 0" />
      <mass
        value="0.468155495274962" />
      <inertia
        ixx="9.06243066774355E-05"
        ixy="-1.32348898008484E-21"
        ixz="1.00671691722841E-12"
        iyy="9.06243066774355E-05"
        iyz="4.30529316694698E-13"
        izz="1.33149137822056E-20" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_right_wheel_2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_right_wheel_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_wheel_right_2"
    type="continuous">
    <origin
      xyz="0.146 -0.0829679051038408 -0.0346340912273031"
      rpy="1.70123451069302 0.836486355878471 0.0970702355963308" />
    <parent
      link="base_link" />
    <child
      link="h_right_wheel_2" />
    <axis
      xyz="0 0 1" />
  </joint>
  <link
    name="h_right_wheel_1">
    <inertial>
      <origin
        xyz="-4.22438230229805E-11 3.60110968822269E-11 0.0306178677327071"
        rpy="0 0 0" />
      <mass
        value="0.468155495297009" />
      <inertia
        ixx="9.06243066737984E-05"
        ixy="-1.05879118406788E-21"
        ixz="5.90970870748764E-13"
        iyy="9.06243066737984E-05"
        iyz="-5.20412499807869E-13"
        izz="7.0176896696916E-21" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_right_wheel_1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_right_wheel_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_wheel_right_1"
    type="continuous">
    <origin
      xyz="0 -0.0829680738553634 -0.0346341059911482"
      rpy="1.67710489610305 -0.606963605871744 -0.0607903310164976" />
    <parent
      link="base_link" />
    <child
      link="h_right_wheel_1" />
    <axis
      xyz="0 0 1" />
  </joint>
  <link
    name="h_left_wheel_2">
    <inertial>
      <origin
        xyz="-5.15928341715544E-11 -1.98986799260226E-11 0.0465382403679345"
        rpy="0 0 0" />
      <mass
        value="0.468155495272803" />
      <inertia
        ixx="9.06243066777683E-05"
        ixy="-1.85288457211878E-22"
        ixz="1.01768252024522E-12"
        iyy="9.06243066777683E-05"
        iyz="4.38647078571242E-13"
        izz="1.37817576332584E-20" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_left_wheel_2.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_left_wheel_2.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_wheel_left_2"
    type="continuous">
    <origin
      xyz="0.146 0.0671082830441516 -0.0332465540892745"
      rpy="1.70172090315557 0.839815899464149 -3.04386814481467" />
    <parent
      link="base_link" />
    <child
      link="h_left_wheel_2" />
    <axis
      xyz="0 0 -1" />
  </joint>
  <link
    name="h_left_wheel_1">
    <inertial>
      <origin
        xyz="-6.27208632253407E-11 9.52182777069766E-12 0.04364857266383"
        rpy="0 0 0" />
      <mass
        value="0.468155495264823" />
      <inertia
        ixx="9.06243066789886E-05"
        ixy="-1.9389113558243E-21"
        ixz="1.25174970524369E-12"
        iyy="9.06243066789886E-05"
        iyz="-1.19839258779808E-13"
        izz="1.77677634451835E-20" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_left_wheel_1.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_left_wheel_1.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_wheel_left_1"
    type="continuous">
    <origin
      xyz="0 0.0699869546901772 -0.0334984052243152"
      rpy="1.65920389521904 0.160636578101035 -3.12741615571841" />
    <parent
      link="base_link" />
    <child
      link="h_left_wheel_1" />
    <axis
      xyz="0 0 -1" />
  </joint>
  <link
    name="h_left_wheel_0">
    <inertial>
      <origin
        xyz="6.54019893797653E-12 -4.40429359649386E-11 0.044879774774279"
        rpy="0 0 0" />
      <mass
        value="0.468155495276783" />
      <inertia
        ixx="9.06243066771557E-05"
        ixy="-6.23363309619962E-21"
        ixz="-3.6827425004458E-14"
        iyy="9.06243066771557E-05"
        iyz="8.24056613336404E-13"
        izz="7.49901175157295E-21" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_left_wheel_0.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://car_design_full/meshes/h_left_wheel_0.STL" />
      </geometry>
    </collision>
  </link>
  <joint
    name="joint_wheel_left_0"
    type="continuous">
    <origin
      xyz="-0.146 0.068760437675547 -0.0333910988899133"
      rpy="-1.84662044837336 1.24503993158855 -0.261989767251398" />
    <parent
      link="base_link" />
    <child
      link="h_left_wheel_0" />
    <axis
      xyz="0 0 -1" />
  </joint>
</robot>

Gazebo launch file: gazebo.launch:

<launch>
  <include
    file="$(find gazebo_ros)/launch/empty_world.launch" >
      <arg name="paused" value="false"/>
      <arg name="use_sim_time" value="true"/>
      <arg name="gui" value="true"/>
      <arg name="headless" value="false"/>
      <arg name="debug" value="true"/>
    </include>
  <node
    name="tf_footprint_base"
    pkg="tf"
    type="static_transform_publisher"
    args="0 0 0 0 0 0 base_link base_footprint 40" />


  <node
    name="spawn_model"
    pkg="gazebo_ros"
    type="spawn_model"
    respawn="false"
    args="-file $(find car_design_full)/urdf/car_design_full.urdf -urdf -model car_design_full"
    output="screen" />
  <node
    name="fake_joint_calibration"
    pkg="rostopic"
    type="rostopic"
    args="pub /calibrated std_msgs/Bool true" />
</launch>

Originally posted by tooght on ROS Answers with karma: 33 on 2018-01-07

Post score: 1


Original comments

Comment by jayess on 2018-01-07:
Welcome! Can you please update your question with a copy and paste of the error that Gazebo is giving you as opposed to linking to a screenshot? Text is searchable, images are not.

Comment by gvdhoorn on 2018-01-07:
In addition to that: I've given you enough karma to embed all your images in your post. Could you please do that?

Comment by tooght on 2018-01-07:
Mr. @jayess I prepared my question. Could you please look at my problem now ?

Comment by jayess on 2018-01-07:
Again, please copy and paste the error. Text is searchable images are not.

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

One of the issues you have, the one that make the model appear collapsed is due to errors in the inertial values. For some reason, when the inertias are not correct , as soon as the model touches the ground and you exert some kind of force in the continuous joints of the wheels, they break and appear in the origin of the model.

You can test this by spawning the model in a zero gravity environment and it won't break any joint. I've made a full video tutorial talking about the issue and explaining how to recalculate your inertal values, at least for basic geometric shapes. Here is the VIDEO ]

Here you have some images of the differences in the inertias for a simple base_link and a wheel:

Colapsed due to wrong Inertias

Bad Inertias Simple

Same urdf except that it has recalculated inertias

Good Inertias Simple

Here I leave a geometric version of your URDF that works and the launch files and python code I used to do this. If you can upload the stl I could update this answer with the complete model as well as maybe add the control.

Inertia Calculator:

#!/usr/bin/env python
"""
Made by TheConstructSim
You can use this script as you please
"""

import math


class InertialCalculator(object):

    def __init__(self):
        print "InertialCaluclator Initialised..."

    def start_ask_loop(self):

        selection = "START"

        while selection != "Q":
            print "#############################"
            print "Select Geometry to Calculate:"
            print "[1]Box width(w)*depth(d)*height(h)"
            print "[2]Sphere radius(r)"
            print "[3]Cylinder radius(r)*height(h)"
            print "[Q]END program"
            selection = raw_input(">>")
            self.select_action(selection)

        print "InertialCaluclator Quit...Thank you"

    def select_action(self, selection):
        if selection == "1":
            mass = float(raw_input("mass>>"))
            width = float(raw_input("width>>"))
            depth = float(raw_input("depth>>"))
            height = float(raw_input("height>>"))
            self.calculate_box_inertia(m=mass, w=width, d=depth, h=height)
        elif selection == "2":
            mass = float(raw_input("mass>>"))
            radius = float(raw_input("radius>>"))
            self.calculate_sphere_inertia(m=mass, r=radius)
        elif selection == "3":
            mass = float(raw_input("mass>>"))
            radius = float(raw_input("radius>>"))
            height = float(raw_input("height>>"))
            self.calculate_cylinder_inertia(m=mass, r=radius, h=height)
        elif selection == "Q":
            print "Selected Quit"
        else:
            print "Usage: Select one of the give options"


    def calculate_box_inertia(self, m, w, d, h):
        Iw = (m/12.0)*(pow(d,2)+pow(h,2))
        Id = (m / 12.0) * (pow(w, 2) + pow(h, 2))
        Ih = (m / 12.0) * (pow(w, 2) + pow(d, 2))
        print "BOX w*d*h, Iw = "+str(Iw)+",Id = "+str(Id)+",Ih = "+str(Ih)

    def calculate_sphere_inertia(self, m, r):
        I = (2*m*pow(r,2))/5.0
        print "SPHERE Ix,y,z = "+str(I)

    def calculate_cylinder_inertia(self, m, r, h):
        Ix = (m/12.0)*(3*pow(r,2)+pow(h,2))
        Iy = Ix
        Iz = (m*pow(r,2))/2.0
        print "Cylinder Ix,y = "+str(Ix)+",Iz = "+str(Iz)

if __name__ == "__main__":
    inertial_object = InertialCalculator()
    inertial_object.start_ask_loop()

car_design_full_geometric_fixed_physics.urdf

<?xml version="1.0"?>
<robot name="car_design_full">  
<material name="material_base_link">
    <color rgba="0.929411765 0.619607843 0.149019608 1"/>
</material>    
<material name="material_sensors">
    <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
</material>    
<material name="material_wheel">
    <color rgba="0.0980392156862745 0.0980392156862745 0.0980392156862745 1"/>
</material>

<link name="base_link">
  <inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.555250122552161" />
  <inertia 
    ixx="0.0018554608262"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000467335519815"
    iyz="0.0"
    izz="0.0023135421773"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.1 0.2 0.01"/>
  </geometry>
  <material name="material_base_link"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.1 0.2 0.01"/>
  </geometry>
</collision>
</link>


<!-- This is for color and physical properties in Gazebo, color won't work with the material tag in gazebo
only for URDF coloring -->
<gazebo reference="base_link">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Yellow</material>
</gazebo>

 <link
    name="imu">
    <inertial>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <mass 
        value="0.154902068029993" />
      <inertia
    ixx="4.03390802161e-05"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000137152872735"
    iyz="0.0"
    izz="0.000161356320865"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.1 0.05 0.025"/>
  </geometry>
  <material name="material_sensors"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
   <box size="0.1 0.05 0.025"/>
  </geometry>
</collision>
</link>


<gazebo reference="imu">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Grey</material>
</gazebo>

<joint name="joint_imu" type="fixed">
<origin xyz="0 -0.05 0.015" rpy="0 0 0" />
<parent link="base_link" />
<child link="imu" />
</joint>


<link
name="h_realsense_0">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.154902068029993" />
  <inertia 
    ixx="3.25939768146e-05"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000161356320865"
    iyz="0.0"
    izz="0.000129407769333"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.1 0.005 0.05"/>
  </geometry>
  <material name="material_sensors"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.1 0.005 0.05"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_realsense_0">
    <kp>1000.0</kp>
    <kd>10.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Grey</material>
</gazebo>


<joint name="joint_realsense_0" type="fixed">
<origin xyz="0 0.1 0.025" rpy="0 0 0" />
<parent link="base_link" />
<child link="h_realsense_0" />
</joint>


<link
name="laser">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="7.15265038550835" />
  <inertia 
    ixx="0.00762949374454"
    ixy="0.0"
    ixz="0.0"
    iyy="0.00762949374454"
    iyz="0.0"
    izz="0.00762949374454"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.08 0.08 0.08"/>
  </geometry>
  <material name="material_laser"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <box size="0.08 0.08 0.08"/>
  </geometry>
</collision>
</link>


<gazebo reference="laser">
    <kp>1000.0</kp>
    <kd>10.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Grey</material>
</gazebo>


<joint
name="joint_lms11_0"
type="fixed">
<origin xyz="0 0.05 0.04" rpy="0 0 0" />
<parent link="base_link" />
<child link="laser" />
</joint>



<!-- RIGHT WHEELS -->
<!-- Back Right Wheel -->
<link
name="h_right_wheel_0">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.46815549529416" />
  <inertia
    ixx="0.000222373860265"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000222373860265"
    iyz="0.0"
    izz="0.000374524396235"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
  <material name="material_wheel"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_right_wheel_0">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Black</material>
</gazebo>


<joint name="joint_wheel_right_0" type="continuous">
<origin xyz="0.065 -0.1 -0.05" rpy="0.0 1.5708 0.0" />
<parent link="base_link" />
<child link="h_right_wheel_0" />
<limit effort="1000000000000000000000.0" velocity="10000000000.0"/>
<axis xyz="0 0 1" />
</joint>



<!-- Front Right Wheel -->
<link
name="h_right_wheel_2">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.46815549529416" />
  <inertia
    ixx="0.000222373860265"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000222373860265"
    iyz="0.0"
    izz="0.000374524396235"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
  <material name="material_wheel"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_right_wheel_2">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Black</material>
</gazebo>


<joint name="joint_wheel_right_2" type="continuous">
<origin xyz="0.065 0.1 -0.05" rpy="0.0 1.5708 0.0" />
<parent link="base_link" />
<child link="h_right_wheel_2" />
<limit effort="1000000000000000000000.0" velocity="10000000000.0"/>
<axis xyz="0 0 1" />
</joint>

<!-- Center Right Wheel -->
<link
name="h_right_wheel_1">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.46815549529416" />
  <inertia
    ixx="0.000222373860265"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000222373860265"
    iyz="0.0"
    izz="0.000374524396235"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
  <material name="material_wheel"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_right_wheel_1">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Black</material>
</gazebo>


<joint name="joint_wheel_right_1" type="continuous">
<origin xyz="0.065 0 -0.05" rpy="0.0 1.5708 0.0" />
<parent link="base_link" />
<child link="h_right_wheel_1" />
<limit effort="1000000000000000000000.0" velocity="10000000000.0"/>
<axis xyz="0 0 1" />
</joint>


<!-- LEFT WHEELS -->

<!-- Back Left Wheel -->
<link
name="h_left_wheel_0">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.46815549529416" />
  <inertia
    ixx="0.000222373860265"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000222373860265"
    iyz="0.0"
    izz="0.000374524396235"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
  <material name="material_wheel"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_left_wheel_0">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Black</material>
</gazebo>


<joint name="joint_wheel_left_0" type="continuous">
<origin xyz="-0.065 -0.1 -0.05" rpy="0.0 1.5708 0.0" />
<parent link="base_link" />
<child link="h_left_wheel_0" />
<limit effort="1000000000000000000000.0" velocity="10000000000.0"/>
<axis xyz="0 0 1" />
</joint>


<!-- Front Left Wheel -->
<link
name="h_left_wheel_2">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.46815549529416" />
  <inertia
    ixx="0.000222373860265"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000222373860265"
    iyz="0.0"
    izz="0.000374524396235"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
  <material name="material_wheel"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_left_wheel_2">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Black</material>
</gazebo>


<joint name="joint_wheel_left_2" type="continuous">
<origin xyz="-0.065 0.1 -0.05" rpy="0.0 1.5708 0.0" />
<parent link="base_link" />
<child link="h_left_wheel_2" />
<limit effort="1000000000000000000000.0" velocity="10000000000.0"/>
<axis xyz="0 0 1" />
</joint>

<!-- Center Left Wheel -->
<link
name="h_left_wheel_1">
<inertial>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <mass 
    value="0.46815549529416" />
  <inertia
    ixx="0.000222373860265"
    ixy="0.0"
    ixz="0.0"
    iyy="0.000222373860265"
    iyz="0.0"
    izz="0.000374524396235"/>
</inertial>
<visual>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
  <material name="material_wheel"/>
</visual>
<collision>
  <origin
    xyz="0 0 0"
    rpy="0 0 0" />
  <geometry>
    <cylinder radius="0.04" length="0.03"/>
  </geometry>
</collision>
</link>


<gazebo reference="h_left_wheel_1">
    <kp>10000000.0</kp>
    <kd>10000000.0</kd>
    <mu1>10.0</mu1>
    <mu2>10.0</mu2>
    <material>Gazebo/Black</material>
</gazebo>


<joint name="joint_wheel_left_1" type="continuous">
<origin xyz="-0.065 0 -0.05" rpy="0.0 1.5708 0.0" />
<parent link="base_link" />
<child link="h_left_wheel_1" />
<limit effort="1000000000000000000000.0" velocity="10000000000.0"/>
<axis xyz="0 0 1" />
</joint>

</robot>

And the launch and spawn files: main.launch

<launch>  
  <arg name="robot" default="machines"/>
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="pause" default="false"/>  <!-- Start Gazebo with a blank world -->
  
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find car_design_full)/worlds/test.world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg pause)"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="headless" value="$(arg headless)"/>
    <env name="GAZEBO_MODEL_PATH" value="$(find simulation_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
  </include>

</launch>

spawn_car_design.launch

    <?xml version="1.0" encoding="UTF-8"?>

<launch>
    
    <include file="$(find spawn_robot_tools_pkg)/launch/spawn_robot_urdf.launch">
    <arg name="x" value="0.0" />
    <arg name="y" value="0.0" />
    <arg name="z" value="0.3" />
    <arg name="roll" value="0"/>
    <arg name="pitch" value="0"/>
    <arg name="yaw" value="0.0" />
    
    <!--
    <arg name="urdf_robot_file" value="$(find car_design_full)/urdf/car_design_full_geometric_testing.urdf" />
    -->
    <arg name="urdf_robot_file" value="$(find car_design_full)/urdf/car_design_full_geometric_fixed_physics.urdf" />
    <arg name="robot_name" value="car_design_full" />
    </include>


</launch>

spawn_robot_urdf.launch

<?xml version="1.0" encoding="UTF-8"?>

<launch>

    <arg name="x" default="0.0" />
    <arg name="y" default="0.0" />
    <arg name="z" default="0.0" />
    <arg name="roll" default="0"/>
    <arg name="pitch" default="0"/>
    <arg name="yaw" default="0.0" />
    
    <arg name="urdf_robot_file" default="" />
    <arg name="robot_name" default="" />

    <!-- This Version was created due to some errors seen in the V1 that crashed GAzebo or went too slow in spawn -->
    <!-- Load the URDF into the ROS Parameter Server -->
    <param name="robot_description" command="cat $(arg urdf_robot_file)" />
    
    <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
    <node name="$(arg robot_name)_urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw) -model $(arg robot_name) -param robot_description"/>
</launch>

test.world

<sdf version="1.5">

<world name="default">


<include>
    <uri>model://sun</uri>
</include>

<include>
    <uri>model://ground_plane</uri>
</include>



</world>
</sdf>

Hope it was useful. Please comment if you have other issues after this.


Originally posted by RDaneelOlivaw with karma: 281 on 2018-01-08

This answer was ACCEPTED on the original site

Post score: 3


Original comments

Comment by tooght on 2018-01-08:
Hi.@RDaneelOlivaw Thank you so much. I have tried the suggestion that you have given, as I reached the maximum length of comments, I could not write a comment. I add below as an answer.

Comment by jayess on 2018-01-08:
What I'm saying is to update your question instead of using an answer. This isn't a forum.

Comment by regenyue on 2019-04-17:
Hi,@RDaneelOlivaw ,I have same error(gazebo model collapse) to display our soldiworks model in gazebo,and i also try your method ,i change our inertia value in our urdf, futhermore we use meshlab to caculate our model inertia.Could you help me to see this problem?Thank you very much

Comment by RDaneelOlivaw on 2019-04-23:
Could you share the code in someway? Its really difficult to help you without enough information

$\endgroup$
0
$\begingroup$

Rosanswers logo

The URDF to SDF conversion removes all fixed joints. One workaround is to replace fixed joints with revolute joints with 0 limits. I suggest using gzsdf to see the results of the URDF to SDF conversion, you'll find instructions here: http://gazebosim.org/tutorials?tut=ros_urdf#VerifyingtheGazeboModelWorks

Take a look at previous questions, such as: https://answers.ros.org/question/106766/problem-at-urdf-to-sdf-conversion-using-gzsdf/


Originally posted by clyde with karma: 1247 on 2018-01-07

This answer was NOT ACCEPTED on the original site

Post score: 2


Original comments

Comment by tooght on 2018-01-08:
thank you @clyde. I will try your suggestions.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.