Problem with "build_serial_chain_from_urdf()"

[66Lau]

Thanks for your work!

I found problem in build_serial_chain_from_urdf(). Specifically, my urdf is about humanoid, which should have for serial chain(left hand, right hand, left leg, right leg).

pelvis_frame
├──── left_hip_roll_link_frame
│ └──── left_hip_yaw_link_frame
│ └──── left_hip_pitch_link_frame
│ └──── left_knee_link_frame
│ └──── left_ankle_link_frame
├──── right_hip_roll_link_frame
│ └──── right_hip_yaw_link_frame
│ └──── right_hip_pitch_link_frame
│ └──── right_knee_link_frame
│ └──── right_ankle_link_frame
└──── torso_link_frame
├──── left_shoulder_pitch_link_frame
│ └──── left_shoulder_roll_link_frame
│ └──── left_shoulder_yaw_link_frame
│ └──── left_elbow_link_frame
└──── right_shoulder_pitch_link_frame
└──── right_shoulder_roll_link_frame
└──── right_shoulder_yaw_link_frame
└──── right_elbow_link_frame

When I trying to load serial chain such as from pelvis_frame to left_ankle_link_frame using
chain_leg = kp.build_serial_chain_from_urdf(urdf_data, "left_ankle_link", "pelvis"), . It still output the whole body tree same as above.

If I replace it with chain_leg = kp.build_serial_chain_from_urdf(urdf_data, "left_ankle_link", "left_hip_roll_link"), it works.
left_hip_roll_link_frame
└──── left_hip_yaw_link_frame
└──── left_hip_pitch_link_frame
└──── left_knee_link_frame
└──── left_ankle_link_frame

Therefore, I guess there are some problems in build_serial_chain_from_urdf function that can not handle multiple child node correctly.