get_link_position_function() happens problem for mobile dual-arm robot

[duISIR]

Hi,

There happens problem in get_link_position_function.
For mobile dual-arm robot, its floating base is called donkey_base. The end-effector links of two arms are called LARM_END_EFFECTOR_grasp and RARM_END_EFFECTOR_grasp, respectively.
The world link is called base_link.

During the running process of the mobile robot, I want to calculate the relative position of the end-effectors in the donkey_base frame. However, their position w.r.t the donkey_base are not correct.
PS. I have already checked the the end-effectors' positions w.r.t. the base link (this is the global frame) are correct.

Maybe the issue happens due to the same probem in in . Or maybe the get_link_transform in model.py happens some problems.

Thank you very much in advance.

[levevictor]

I am having similar issues when I want to get the transform of a link relative to another link which is note the world.
I suspect the problem to come from the last line of the get_link_transform (or get_link_transform_function) :
return T_L_W @ invt(T_B_W) (https://github.com/cmower/optas/blob/master/optas/models.py#L886)
where it should be instead
return invt(T_B_W) @ T_L_W
i.e. apply the transform from the base_link frame to the world frame invt(T_B_W), then apply the transform from the world frame to the link frame T_L_W.

Meanwhile, the workaround I use is to calculate the correct transform T myself as follows:

# self here is my instance of the optas.RobotModel class
T_L_W = self.get_global_link_transform(link, q)
T_B_W = self.get_global_link_transform(base_link, q)
T = optas.invt(T_B_W) @ T_L_W

[joaomoura24]

@levevictor, I would suggest to create a branch, fix this, and then pull request. You can put me as reviewer.