Question regarding the definition and calculation process of FK

[Shikun Ban]

Hello! I really like your work and your code helped me a lot! I have some questions regarding the definition and calculation process of FK! I think the FK that your code calculates is a map from Link objects to their poses relative to the robot’s base link as 4x4 homogenous transform matrices. Am I right?
Because I'm currently working on a pipeline that takes in joint values and calculates joints/keypoints' 3D locations (in the camera coordinate frame), which involves using your FK matrices. But there's some calculation error so I just want to check if my understanding of your FK calculated here is correct.

And also ,my geometry calculation process:
I used your 4x4 FK matrices as representation of link-to-base pose, and I currently have the base-to-camera pose (also represented in 4x4 matrix). So by doing matrix multiplication, I now have the link-to-camera pose, (we can denote it as matrix T). My goal is to calculate the keypoints' location in the camera coordinate frame, which I think can be calculated by:
T *(x,y,z,1)^T ( matrix T right-multiplied by (x,y,z,1), in which (x,y,z) means the keypoints' location in the link coordinate frame)
Moreover, I think the origin of the link_frame is placed exactly at the position of the joint, right? (conventional thinking). In my case, my desired keypoints are equal to the joints, so that the (x,y,z) above (keypoints' location in the link coordinate frame), should be (0,0,0).

I'm a bit afraid that I got it wrong somewhere. Through the whole process I have mainly 2 questions that I would like to check with you, which is the definition of your FK and whether your origin of the link_frame is placed exactly at the position of the joint.

Thank you!!!!

[Johnson Zhong]

Yes FK returns transforms that are wrappers around 4x4 homogenous transform matrices (let's call them H) that maps points in link frame to world frame (via left multiplication Hx). Just to be absolutely clear, if a point has homogenous coordinate x=(0,0,0,1), and H is the return from for example:

    th = torch.tensor([0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0], device=d)
    tf = chain.forward_kinematics(th, end_only=False)
    H = tf['lbr_iiwa_link_7'].get_matrix()

Hx will be the robot's base frame coordinate of the lbr_iiwa_link_7 link's origin. I'll add more explanation of the transforms to the method docstrings.

Note that you can specify a world transform in forward_kinematics which should be the transform of the robot base (world)B(base). If you omit this, the returned transforms are with respect to the base frame while if you supply this they will be with respect to the world.

Joints technically don't have positions or any physical manifestation. They define how two links can transform with respect to each other (usually in a parent-child relationship). You can probably use the parent link's frame origin as the "position" of the joint.

For keypoints, you can probably work things out by transforming (0,0,0,1) around in different frames, but to be clear we have the FK transform: (base)H(link). It expects link coordinates on the right side and the output will be a base frame coordinate. We also know the transform of the robot base with respect to the world (world)B(base). You then need to have your camera intrinsic matrix let's say (world)K(camera). To get link to camera frame, we have (camera)K^{-1}BH(link)

[Shikun Ban]

Thank you a lot for your explanation!!! I'm now a lot clearer than before!!
Actually, I'm currently using the DREAM dataset and I may have found that most of the link coordinates is pretty much just (0,0,0,1) but some link coordinates have a seemingly constant small deviation value. But I think the error that I encountered should be attributed to the DREAM setup, which is not relevant to your code. I'm currently digging up more info on my findings!!
Thanks again!!!!