0
$\begingroup$

I am trying to simulate a robot I do not want fixed to the Gazebo world I am spawning it into. However, when I make the joint between my base link and the world floating, I get unexpected behavior in the way children frames of reference are established.

To preface, here is all the relevant XACRO

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">

  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/base_wide.xacro" />
  <xacro:include filename="/home/jackfrye/Documents/Robotics/workspace/src/frye_arm_1/urdf/polou_4801.xacro" />

  <link name="world"></link>

  <joint name="world_to_base" type="floating">
      <parent link="world" />
  <child link="a_base_wide" />
  <origin  rpy="0 0 0" xyz="0 0 0" />
  </joint>

  <xacro:base_wide prefix="a" x="0.0" y="0.0" z="0.374" />

  <joint name="base_to_motor_body" type="fixed">
      <parent link="a_base_wide" />
      <child link="a_robot_body" />
      <origin  rpy="0 0 0" xyz="0 0 .359" />
  </joint>

  <xacro:polou_4801 prefix="a" x="0.0" y="0.0" z="0.0" phi="0.0" theta="0.0" psi="0.0" />
</robot>

base_wide.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="base_wide" params="prefix x y z">
  
  <link name="${prefix}_base_wide">
    <visual>
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0"/>    
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/base_widen/base_widen_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="${x} ${y} ${z}" rpy="0 0 0"/>
        <mass value="131.0971" />
        <inertia ixx="0.1811" ixy="0.0" ixz="0.0" iyy="0.1809" iyz="0.0002" izz="0.18" />      
    </inertial>     
  </link>
  </xacro:macro>
</robot>

polou_4801.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="frye_arm_1">
  <xacro:macro name="polou_4801" params="prefix x y z phi theta psi">
  
    <link name="${prefix}_robot_body">
    <visual>
      <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
      <geometry>
        <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />    
      <geometry>
          <mesh filename="file://file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_body/polou_4801_body_x10_no_wire.dae"/>
      </geometry>
    </collision>
    <inertial>
        <origin xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" />
        <mass value="94" />
        <inertia ixx="3.4438" ixy="0.0" ixz="0.0" iyy="3.4438" iyz="0.0" izz="0.7338" />      
    </inertial>     
  </link>

  <joint name="${prefix}_body_to_rotor" type="revolute">
      <parent link="${prefix}_robot_body" />
      <child link="${prefix}_robot_rotor" />
      <axis xyz="0 0 1" />
<!--      <origin  xyz="${x} ${y} ${z}" rpy="${phi} ${theta} ${psi}" /> -->
         <origin  xyz="0 0 0.374" rpy="0 0 0" /> 
      <limit lower="0" upper="${4*pi}" effort="10" velocity="3"/>
  </joint>
  
  <link name="${prefix}_robot_rotor">
    <visual>
<!--      <origin xyz="${x} ${y} ${z+0.75}" rpy="${phi} ${theta} ${psi}" /> -->
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" /> 
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
<!--      <origin xyz="${x} ${y} ${z+0.75}" rpy="${phi} ${theta} ${psi}" /> -->
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" /> 
      <geometry>
          <mesh filename="file:///home/jackfrye/Documents/Robotics/FryeArm1/Mechanical/ROS2_Prep/polou_4801_shaft/polou_4801_shaft_x10.dae"/>
      </geometry>
    </collision>
    <inertial>
<!--      <origin xyz="${x} ${y} ${z+0.75}" rpy="${phi} ${theta} ${psi}" /> -->
      <origin xyz="${x} ${y} ${z}" rpy="0 0 0" /> 
        <mass value=".05" />
        <inertia ixx="0.00000" ixy="0.0" ixz="0.0" iyy="0.0000" iyz="0.0" izz="0.0000" />      
    </inertial>       
   </link>
  </xacro:macro>

</robot>

Now in the top level for the joint "world_to_base", when I used floating, which is the setting I want to use, it seems like the transforms are not being done as expected. If I say I want to make a joint at 0.374 +Z, that means I want that joint at +.374 from the parent's frame of reference. If I create a floating joint the move the child +Z .359, that should create the frame of reference from which the joint is created. In that case, the joint would then be at +.359+.374 in the "world" frame. When I do a floating joint, I do not see this behavior.

enter image description here

However, when I used fixed for the connection between my base link and world, I do see this relationship in the link-joint tree.

enter image description here

enter image description here

It makes it difficult to write portable XACRO if I cannot assume that my make first joint in the XACRO (eg the motor body joint) is going to be offset WRT to its parent not and not world. Maybe I just need to refactor and add logic into the polou xacro that takes into account that all joints are being layed out wrt the world frame, but I would prefer not to and just have it calculate like it does for the "fixed" case.

$\endgroup$

2 Answers 2

1
$\begingroup$

I think there is an issue with the URDF-to-SDFormat converter in libsdformat's parser_urdf.cc file since it currently ignores both floating and planar joint types. I've opened the following feature request to sdformat to properly parse the floating joint origin tags.

$\endgroup$
0
$\begingroup$

In my experience, I've only seen the pattern where you add a world link to the URDF to affix the robot to the world. If you want it to be floating, you don't need the world link at all, just start from a_base_wide. There is no floating joint in SDF since that is equivalent to not having a joint (https://answers.gazebosim.org/question/14728/what-is-a-floating-joint-physically/?answer=14737#post-id-14737). If you still need to offset the whole robot when it's spawned, you can use the <gazebo> tag to directly insert a <pose> tag to the resulting model.

  <gazebo>
    <pose>1 0 0  0 0 0</pose>
  </gazebo>

I haven't tested that so it may or may not work. Alternatively, you can change your launch script to spawn the robot at a desired pose.

$\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.