Problems while specifying root link

[llliuxiao]

My robot structure is:
"etc -> torso_link2 -> torso_joint3 -> torso_link3 -> left_arm_base_joint -> left_arm_base_link -> left_arm_joint1 -> left_arm_link1 -> etc".

pk.build_serial_chain_from_urdf(urdf, end_link_name="left_arm_link1", root_link_name="torso_link3")

Now I want to calculate the relative transformation between left_arm_lin1 and torso_link3, but I was asked to provide the joint position of torso_joint3.

What is the correct usage for my requirement?

Thanks~

[LemonPi]

Hi you'll need to specify the joint values (position) of each joint. You can then do something like:

# specify joint values (can do so in many forms)
th = [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0, 0.0]
# do forward kinematics to get transforms to each of the links
ret = chain.forward_kinematics(th, end_only=False)

# from base to each frame
base_to_arm = ret['left_arm_link1']
base_to_torso = ret['torso_link3']
arm_to_torso = torch.linalg.inv(base_to_arm) @ base_to_torso