How to move the robot arm in straight line?

edit

If I'm not mistaken, both the default driver (abb_driver) and the KTH YuMi adaptation work in joint space only. So straightlines will require some form of inverse kinematics and will be played out using joint space motion primitives. That means no straight lines.

These drivers are also more geared towards using them with on-line applications that generate motions based on sensor data and need low-level control or execute "complex" joint space trajectories. Your use (simple MoveL) is not properly supported.

As to your specific problem:

compute the Cartesian path but [it is] not working well with me, specially when moving both arms.

computeCartesianPath(..) only interpolates vectors of geometry_msgs/Pose, it doesn't have any concept of "arm". I'm not sure how you used it exactly.

Perhaps you could take a look at abb_librws. It is a C++ library that allows you to directly interface with the Robot Web Services running on the IRC5 controller (RW6 required, but YuMi has that).

Among other things, it supports:

• Reading/writing of RAPID data.
• Starting/stopping/resetting the RAPID program.
• Uploading/downloading/removing files.

Perhaps using a combination of these could work.

Unfortunately its companion library abb_libegm cannot be used with YuMi yet.