What does <axis><xyz expressed_in="some_frame"> mean, and how can I check my math with Gazebo?

Question

Using SDFormat 1.7 I'm trying to use <joint><axis><xyz expressed_in="some_frame"> and check my understanding using Gazebo 11, but I'm not seeing the results I was expecting. What am I doing wrong?

If I understood @azeey's explanation correctly, I think he said <xyz expressed_in="F"> resolves to a vector in the joint frame that is parallel to the given vector in frame F. (made a diagram of what I think he said below)

I have an SDFormat XML file with a joint with an axis in a frame, and I'm trying to check it by calculating what the axis vector should be using igntion-math and visualize it in Gazebo 11. I thought I would visualize it by adding a redundant joint at the same spot without using expressed_in. I was expecting that if I got the math and ignition-math API calls correct then the visualized joint axes would be overlapping; however, the yellow axes are close, but not quite overlapping.

Here is the SDFormat XML

<?xml version='1.0' encoding='utf-8'?>
<sdf version="1.7">
  <model name="joint_revolute_axis_in_frame">
    <frame name="some_frame">
      <pose>0.05 0.1 0.2 0.1 0.2 0.3</pose>
    </frame>
    <link name="link_1">
      <visual name="link_1_visual">
        <geometry>
          <box>
            <size>0.1 0.2 0.4</size>
          </box>
        </geometry>
      </visual>
      <collision name="link_1_collision">
        <geometry>
          <box>
            <size>0.1 0.2 0.4</size>
          </box>
        </geometry>
      </collision>
      <inertial>
        <mass>12.3</mass>
        <inertia>
          <ixx>0.205</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.17425</iyy>
          <iyz>0</iyz>
          <izz>0.05125</izz>
        </inertia>
      </inertial>
    </link>
    <link name="link_2">
      <pose>0.1 0 0.1 0 0 0</pose>
      <visual name="link_2_visual">
        <geometry>
          <box>
            <size>0.1 0.2 0.3</size>
          </box>
        </geometry>
      </visual>
      <collision name="link_2_collision">
        <geometry>
          <box>
            <size>0.1 0.2 0.3</size>
          </box>
        </geometry>
      </collision>
      <inertial>
        <mass>1.23</mass>
        <inertia>
          <ixx>0.013325</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.01025</iyy>
          <iyz>0</iyz>
          <izz>0.005125</izz>
        </inertia>
      </inertial>
    </link>
    <joint name="joint_revolute" type="revolute">
      <parent>link_1</parent>
      <child>link_2</child>
      <axis>
        <xyz expressed_in="some_frame">0.1 1.23 4.567</xyz>
        <limit>
          <lower>-1.5</lower>
          <upper>1.5</upper>
        </limit>
      </axis>
    </joint>
    <!-- redundant joint for checking math -->
    <joint name="joint_revolute2" type="revolute">
      <parent>link_1</parent>
      <child>link_2</child>
      <axis>
        <xyz>0.75246900000000005 1.036583 4.5540830000000003</xyz>
        <limit>
          <lower>-1.5</lower>
          <upper>1.5</upper>
        </limit>
      </axis>
    </joint>
  </model>
</sdf>

And here is the math I used to try to calculate the resolved axis value.

const ignition::math::Pose3d model_to_frame_in_model{0.05, 0.1, 0.2, 0.1, 0.2, 0.3};
const ignition::math::Pose3d model_to_child_in_model{0.1, 0, 0.1, 0, 0, 0};
const ignition::math::Pose3d frame_to_child_in_frame =
  model_to_child_in_model - model_to_frame_in_model;
const ignition::math::Pose3d child_to_joint_in_child{0, 0, 0, 0, 0, 0};
const ignition::math::Pose3d frame_to_joint_in_frame =
  child_to_joint_in_child + frame_to_child_in_frame;

const ignition::math::Vector3d axis_in_frame{0.1, 1.23, 4.567};
const ignition::math::Vector3d axis_in_joint =
  frame_to_joint_in_frame.Inverse().Rot().RotateVector(axis_in_frame);

---

Originally posted by sloretz on Gazebo Answers with karma: 558 on 2020-10-08

Post score: 0

Answer 1

I think your calculation is correct. Unfortunately, you might have uncovered a bug in the way Gazebo11 visualizes axes. Setting the the rotation of some_frame to 1.57, 0.2, 0.3 shows the issue more clearly.

BTW, I think we are moving away from using the - and + operators in favor of the * operator for ignition::math::Pose3d.

---

Originally posted by azeey with karma: 704 on 2020-10-08

This answer was ACCEPTED on the original site

Post score: 1