URDF to SDF Frame of Reference Calculation for Base Joint Fixed vs Floating
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 "worldtobase", 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.
However, when I used fixed for the connection between my base link and world, I do see this relationship in the link-joint tree.
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.
Asked by jfrye11 on 2023-02-24 19:26:05 UTC
Answers
Answered at https://robotics.stackexchange.com/a/24488/31574
Asked by azeey on 2023-03-02 00:05:20 UTC
Comments