search

cmower / optas

OpTaS: An optimization-based task specification library for trajectory optimization and model predictive control.
https://cmower.github.io/optas/
Other
99 stars 14 forks source link

For two-arm robot, there happens incorrect geometric and analytical Jacobians #139

Closed duISIR closed 1 year ago

duISIR commented 1 year ago

Hello, Optas is a great package which makes optimization problems become very convenient. During the use of a two-arm robot nextage Robot, there may happen issues on the calculation of Jacobians. In geometric Jacobians of two arm end-effectors, translational and angular parts may be not correct, and for the analytical Jacobian, the translational part may be not correct. For example, the right-arm end-effector velocity is independent of the left-arm joints. The left-arm end-effector velocity is independent of the right-arm joints. However, the corresponding independent part in Jacobians are not zero currently. The angular parts of geometric Jacobians for right and left arm end-effectors are not different currently. Thank you very much.

cmower commented 1 year ago

Hi @duISIR, thank you for your kind words, it's great to hear optas is useful for you!

I assume you are using the URDF found here?

Could you please share the code which recreates the issue you observe.

duISIR commented 1 year ago

Hi @cmower , we use the code here . For example, in action_server_cmd_pose.py, there is a robot model called self.right_arm, we can build two groups of Jacobian functions: self.Jac_fnc_Right_analytical = self.right_arm.get_global_link_analytical_jacobian_function(link=self._link_ee_right) self.Jac_fnc_Left_analytical = self.right_arm.get_global_link_analytical_jacobian_function(link=self._link_ee_left) and self.Jac_fnc_Right_geometrical = self.right_arm.get_global_link_geometrical_jacobian_function(link=self._link_ee_right) self.Jac_fnc_Left_geometrical = self.right_arm.get_global_link_geometrical_jacobian_function(link=self._link_ee_left) We can find that the corresponding part in the Jacobian of right-arm end-effector's velocity is not independent of the left-arm joints， vice versa.

In addition, we can find that the angular parts of two geometrial jacobians are the same which may be not correct.

joaomoura24 commented 1 year ago

I solved this issue in #141. I added a condition to check if joint belongs to the correct chain. @duISIR , can you please pull from my repo and change to the branch fix-139 to check in the solution?

joaomoura24 commented 1 year ago

As mentioned above, this was solved with #141.

duISIR commented 1 year ago

Thank you very much.