0
$\begingroup$

I'm relatively new to ROS. I started with raw URDF exported from solidworks. Then created moveit_config package by moveit_setup_assistant. Here are the related files: URDF file:

<robot name="prochesta_arm">
    <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
    </material>
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1.0"/>
    </material>
    <material name="grey_2">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
    </material>
    <material name="pink">
        <color rgba="1.0 0.5 0.5 1.0" />
    </material>
    <material name="blue">
        <color rgba="0.0 0.4 1.0 1.0" />
    </material>
    <material name="green">
        <color rgba="0.0 1.0 0.5 1.0" />
    </material>
    <link name="base_bar">
        <!--inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="0.392347184025621" />
            <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial-->
        <visual>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.01 0.01 0.1"/>
            </geometry>
            <material name="grey"/>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.01 0.01 0.1"/>
            </geometry>
        </collision>
    </link>
    <joint name="base_plane__ArmBase" type="fixed">
        <origin rpy="0.0 0.0 0.0" xyz="0.0546412131471822 0.0 0.11" />
        <parent link="base_bar" />
        <child link="ArmBase" />
        <axis xyz="0.0 0.0 0.0" />
        <limit lower="0.0" upper="0.0" effort="1000.0" velocity="50.0"/>
    </joint>
    <joint name="base_bar__base_plane" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="base_plane"/>
        <child link="base_bar"/>
        <axis xyz="0.0 0.0 0.0"/>
        <limit lower="0.0" upper="0.0" effort="0.0" velocity="0.0"/>
    </joint>
    <link name="base_plane">
        <!--inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="0.392347184025621" />
            <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial-->
        <visual name="">
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="1.5 1.5 0.02"/>
            </geometry>
            <material name="grey"/>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="1.5 1.5 0.02"/>
            </geometry>
        </collision>
    </link>
    <link name="ArmBase">
        <inertial>
            <origin rpy="0 0 0" xyz="-0.0546412131471822 0.0142569733044021 0.0105265889786102" />
            <mass value="0.392347184025621" />
            <inertia ixx="0.00037313359562078" ixy="-3.88508187746182E-07" ixz="-9.7227823468437E-06" iyy="0.000612254064917396" iyz="6.685567634577E-07" izz="0.000292429195752973" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/ArmBase.STL" />
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/ArmBase.STL" />
            </geometry>
        </collision>
    </link>
    <link name="BaseShaft">
        <inertial>
            <origin rpy="0 0 0" xyz="-0.00124611489784311 -0.108184816547343 -1.18094289902615E-09" />
            <mass value="0.172603193441375" />
            <inertia ixx="0.000449963539893855" ixy="2.29673976828531E-06" ixz="4.03285265163475E-09" iyy="0.000436230277248471" iyz="-5.17320253939005E-12" izz="0.000511313152396509" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/BaseShaft.STL" />
            </geometry>
            <material name="pink"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/BaseShaft.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="ArmBase__BaseShaft" type="revolute">
        <origin rpy="-1.5708 0 -1.5708" xyz="-0.075617 0.021609 0" />
        <parent link="ArmBase" />
        <child link="BaseShaft" />
        <axis xyz="0 1 0" />
        <limit effort="1000" lower="-3.1416" upper="3.1416" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Shoulder">
        <inertial>
            <origin rpy="0 0 0" xyz="0.00251674474561527 0.170121527752926 0.00159696012847385" />
            <mass value="0.100791466963889" />
            <inertia ixx="0.000469966041530564" ixy="-5.89764046954074E-06" ixz="5.65207828543954E-09" iyy="7.13384967018999E-05" iyz="-3.44218965703515E-07" izz="0.000512104689605307" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Shoulder.STL" />
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Shoulder.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="BaseShaft__Shoulder" type="revolute">
        <origin rpy="1.5535 1.556 -1.5873" xyz="-0.02 -0.1456 0" />
        <parent link="BaseShaft" />
        <child link="Shoulder" />
        <axis xyz="-0.99989 0.01479 0.00025546" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-1" upper=".5" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Elbow">
        <inertial>
            <origin rpy="0 0 0" xyz="-4.98394797654456E-11 -3.62130138409911E-05 -0.17234820422493" />
            <mass value="0.155859341630008" />
            <inertia ixx="0.000268369583946352" ixy="2.061494260678E-10" ixz="-6.45772822678737E-11" iyy="0.000298530192977532" iyz="-4.44744351270454E-05" izz="0.000181542359930575" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Elbow.STL" />
            </geometry>
            <material name="pink"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Elbow.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Shoulder__Elbow" type="revolute">
        <origin rpy="-2.8163 -0.00025546 3.1268" xyz="0.0051765 0.34996 0.00027318" />
        <parent link="Shoulder" />
        <child link="Elbow" />
        <axis xyz="1 0 0" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-0.5" upper=".7837" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Yaw">
        <inertial>
            <origin rpy="0 0 0" xyz="-0.00438644516368353 0.0559409791205142 -0.0570850114857289" />
            <mass value="0.296477336396768" />
            <inertia ixx="0.000492376543215998" ixy="-1.09052812794105E-05" ixz="-2.66243025248788E-05" iyy="0.000302345188534722" iyz="-2.34599297023637E-05" izz="0.000282182542169169" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Yaw.STL" />
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Yaw.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Elbow__Yaw" type="revolute">
        <origin rpy="-0.0016492 -0.098555 0.033437" xyz="0 -0.098331 -0.29083" />
        <parent link="Elbow" />
        <child link="Yaw" />
        <axis xyz="0 0.94732 -0.3203" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-1.5708" upper="1.5708" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Pitch">
        <inertial>
            <origin rpy="0 0 0" xyz="0.00193637684331768 -0.00113094288383903 -0.00473550556070457" />
            <mass value="0.245672351084059" />
            <inertia ixx="0.000112445414870033" ixy="3.7801113676169E-06" ixz="1.13310562352918E-05" iyy="0.000222075669730552" iyz="8.19497975087401E-07" izz="0.000225909542391733" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Pitch.STL" />
            </geometry>
            <material name="pink"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Pitch.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Yaw__Pitch" type="revolute">
        <origin rpy="-0.095533 0.0036424 0.0092858" xyz="-0.013295 0.024778 -0.14277" />
        <parent link="Yaw" />
        <child link="Pitch" />
        <axis xyz="0.99459 -0.033268 -0.098395" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-1.5708" upper="1.5708" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="Roll">
        <inertial>
            <origin rpy="0 0 0" xyz="0.013346395763443 0.026611833443925 0.0965457711475227" />
            <mass value="0.219051002803153" />
            <inertia ixx="0.000155851657200548" ixy="-1.61881929035119E-05" ixz="1.50471506253359E-05" iyy="0.000101493190927247" iyz="4.64669877102981E-05" izz="0.000174715266607372" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Roll.STL" />
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/Roll.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Pitch__Roll" type="revolute">
        <origin rpy="-2.9699 0.21413 -0.25831" xyz="0 0 0" />
        <parent link="Pitch" />
        <child link="Roll" />
        <axis xyz="0.16462 0.40474 0.89949" />
        <!-- limits have been calculated manually-->
        <limit effort="1000" lower="-3.1416" upper="3.1416" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <link name="LeftClaw">
        <inertial>
            <origin rpy="0 0 0" xyz="0.0160548042850929 0.363052679901909 -0.277299510990579" />
            <mass value="0.0710508103277403" />
            <inertia ixx="6.33292286188025E-05" ixy="-7.37244945176046E-06" ixz="-1.6248611953797E-05" iyy="5.934165926219E-05" iyz="-1.6824179025353E-05" izz="2.45890867032075E-05" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/LeftClaw.STL" />
            </geometry>
            <material name="green"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/LeftClaw.STL" />
            </geometry>
        </collision>
    </link>
    <link name="RightClaw">
        <inertial>
            <origin rpy="0 0 0" xyz="0.0160548042897368 0.363052680008611 -0.277299511621476" />
            <mass value="0.0710508080215541" />
            <inertia ixx="6.33292264052498E-05" ixy="-7.37244969866602E-06" ixz="-1.62486114309369E-05" iyy="5.9341657110105E-05" iyz="-1.68241784848982E-05" izz="2.45890872002616E-05" />
        </inertial>
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/RightClaw.STL" />
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://prochesta_arm/meshes/RightClaw.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="Joint_LeftClaw" type="prismatic">
        <origin rpy="0 0 0" xyz="-0.0088606 -0.28846 0.43314" />
        <parent link="Roll" />
        <child link="LeftClaw" />
        <axis xyz="-0.96885 0.23743 0.070474" />
        <limit effort="1000" lower="0" upper="0.021" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>
    <joint name="Joint_RightClaw" type="prismatic">
        <origin rpy="0.86688 -0.30065 3.0016" xyz="0.099182 0.50791 0.05503" />
        <parent link="Roll" />
        <child link="RightClaw" />
        <axis xyz="0.96885 -0.23743 -0.070474" />
        <limit effort="1000" lower="-0.021" upper="0" velocity="50" />
        <dynamics damping="10.0" friction="10.0"/>
    </joint>


    <transmission name="trans_ArmBase__BaseShaft">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="ArmBase__BaseShaft">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="ArmBase__BaseShaft_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_BaseShaft__Shoulder">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="BaseShaft__Shoulder">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="BaseShaft__Shoulder_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Shoulder__Elbow">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Shoulder__Elbow">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Shoulder__Elbow_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Elbow__Yaw">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Elbow__Yaw">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Elbow__Yaw_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Yaw__Pitch">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Yaw__Pitch">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Yaw__Pitch_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Pitch__Roll">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Pitch__Roll">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Pitch__Roll_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Joint_LeftClaw">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint_LeftClaw">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Joint_LeftClaw_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_Joint_RightClaw">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint_RightClaw">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="Joint_RightClaw_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_base_plane__ArmBase">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="base_plane__ArmBase">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="base_plane__ArmBase_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_base_bar__base_plane">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="base_bar__base_plane">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="base_bar__base_plane_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <gazebo>
        <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
            <robotNamespace>/</robotNamespace>
            <legacyModeNS>true</legacyModeNS>
        </plugin>
        <!--plugin filename="libgazebo_ros_joint_state_publisher.so" name="joint_state_publisher">
            <robotNamespace>/</robotNamespace>
            <jointName>
                ArmBase__BaseShaft, 
                BaseShaft__Shoulder, 
                Elbow__Yaw, 
                Pitch__Roll,
                Shoulder__Elbow,
                Yaw__Pitch,
                Joint_LeftClaw, 
                Joint_RightClaw
            </jointName>
        </plugin-->
    </gazebo>
</robot>

gazebo.launch file:

<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
  <arg name="world_name" default="worlds/empty.world"/>
  <arg name="paused" value="true"/>
  <arg name="gui" value="$(arg gazebo_gui)"/>
</include>

<!-- send robot urdf to param server -->
<param name="robot_description" command="xacro  '$(find mybot)/urdf/mybot.urdf.xacro'" />

<!-- unpause only after loading robot model -->
<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
<!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
<arg name="world_pose" value="-x 0 -y 0 -z 0" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
  respawn="false" output="screen" />

<!-- Load joint controller parameters for Gazebo -->
<rosparam file="$(find mybot_moveit_config)/config/gazebo_controllers.yaml" />
<!-- Spawn Gazebo ROS controllers -->
<node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
<!-- Load ROS controllers -->
<include file="$(dirname)/ros_controllers.launch"/>

After launch the error I get:

[ERROR] [1681988267.705543000]: This robot has a joint named "base_plane__ArmBase" which is not in the gazebo model.
[FATAL] [1681988267.705556000]: Could not initialize robot simulation interface

...

[WARN] [1681988296.560628, 28.000000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[WARN] [1681988296.561593, 28.001000]: Controller Spawner couldn't find the expected controller_manager ROS interface.

The gazebo_ros_controller plugin doesn't load. I have a similar arm which I have set up exactly the same way and that works expectedly whereas this one just falls down in the gazebo.

I have found similar problems in this forum and over the internet, but none of them works.

[Edit] Here is the github repository for both the robots. mybot is the one that runs successfully. GitHub

$\endgroup$

2 Answers 2

1
$\begingroup$

Try to write your robot name inside gazebo plugin in robotNamespace

$\endgroup$
2
  • $\begingroup$ Welcome to Robotics Unai Granados Ormaetxea. Thanks for your answer but we are looking for comprehensive answers that provide some explanation and context. Very short answers cannot do this, so please edit your answer to explain why it is right, ideally with citations. Answers that don't include explanations may be removed. See How to Answer for more info. $\endgroup$
    – Ben
    Apr 24, 2023 at 23:39
  • $\begingroup$ @unai-granados-ormaetxea Tried this. The namespace was not required in the other robot that I managed to run successfully. Please see the github repository added in the edit. $\endgroup$
    – Tangent
    Apr 25, 2023 at 9:11
1
$\begingroup$

I found the solution.

Turns out

This robot has a joint named "base_plane__ArmBase" which is not in the gazebo model.

This message was the key. I have set <transmission> tag for all the joints in my URDF. But gazebo treats every links that have fixed joint with each other as a single rigid body. Thus base_plane__base_bar & base_bar__ArmBase joints were not present in the robot model. So gazebo couldn't figure out where to set the hardware interface. This is why the simulation interface was not running.

So I removed the <transmission> tag for both of this joints and it runs expectedly.

$\endgroup$

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.