I can not control my 2 ur5 robots

[LuciAnge]

Hi, I used relaxedIK to control just one ur5 and I got excellent results. Now I'd like to use relaxedIK with a bimanual robot (two ur5 robots as in the figure above).

As the first time (with only one ur5) I run the start_here.py in a ROS Melodic enviroment using Python2.7 to generate all necessary files to use relaxed_ik_core. Then, I transfered all the files into a ROS noetic enviroment and I tried to use them with relaxed_ik_ros1. Unfortunatelly somenthing dosen't work! When I published on /relaxed_ik/ee_pose_goals topic a delta equal to zero the robot should remane in its initial pose, but it moves itself (as you can see in the following figure).

I tried to fix the problem, but I failed.

These are the files I used: collision_2ur5.yaml, 2ur5.urdf, start_here.py

I hope you can help me to figure out which is the problem and how to solve it. Thank you for your time in advance.

[djrakita]

This looks like a great start on the two arm setup. Nothing is jumping out at me in terms of obvious errors - I'll look into this ASAP. Just out of curiosity, did you try training the collision neural network more than once? If you only tried once, it may be worth trying again just to see if that happens to fix things. Sometimes the learning process can be a bit off, especially for multi-chain robots. Our ongoing research is getting around this issue.

[LuciAnge]

Thank you a lot for your quick reply. I just tried to train the neural network again. In the figure there is the output shown in the terminal. I don't know if the value of "loss" gives some information about the success of the training process. Unfortunately during the simulation nothing seems to change.

[LuciAnge]

Update Trying to figure out where the problem was, I noticed that every time I published on /relaxed_ik/ee_pose_goals topic a delta equal to zero the robot, instead of remaining in its pose, moved. In particular each arm moved as the initial pose of each tool was rotated of pi around the z-axis of the fixed_frame. For this reason I modified the info file automatically generated by changing the rot_offset of the first fixed joint with a rotation around the z-axis. I did this for the fixed joint of each arm. I changed only the info file, I did not change the urdf. Surprisingly this seems to fix the problem: when I published a delta equal to zero the robot didn't move. When I tried to move the robot I realized that each end-effector had a sort of new fixed frame. The dx_tool0 moved respect to dx_base and the sx_tool0 moved respect to sx_base. I know this doesn't have much sense. The robot now is able to follow a trajectory but it doesn't detect (and avoid) all the collisions, so the algorithm remains unusable. I hope this information can help you to find a real solution. Thank you for your time.

[LuciAnge]

Update I sorted out what the problem was. The algorithm was not able to reconstruct the correct starting position of the two end-effectors. This was because during the reconstruction the fixed rotation between the fixed_base_base_link_joint and the shoulder_pan_joint was missing. I slightly changed the code (arm.rs) to solve this problem. I'm not sure my solution works for every robot. This is the code: arm.rs