Cartesian Pose Control as directly supported by Panda (#39)

[justagist]

@justagist Hi, thank you very much for providing the panda interface.

I found a weird problem when I use provided demo "task_space_contrl_with_fri"(https://github.com/justagist/panda_simulator/blob/melodic-devel/panda_simulator_examples/scripts/task_space_control_with_fri.py). As you provided the controller, based on the position error in cartesian space, using a PD controller to calculate the desired force in cartesian space. Then use the torque controller in the joint space. I found it can not accurately control the cartesian pose by this controller. For example, when I only track motion （constant velocity）in the X direction (I set the delta_pos[0] =0.01 delta_pos[1] =0.0 delta_pos[2] =0.0 in the control_thread(). ).I expected the robot to only move along the X direction, but the robot also moves in the Z direction (the position in Z decreases). When I test on other directions, it has the same issue. Both in the simulator and the real robot, the controller has the same problems. Thank you very much!

_Originally posted by @siweiyong in https://github.com/justagist/panda_simulator/discussions/39#discussioncomment-1041954_

[justagist]

Hi @siweiyong, There are several things that could be going wrong, but my initial suggestion is not to do this: I set the delta_pos[0] =0.01 delta_pos[1] =0.0 delta_pos[2] =0.0 in the control_thread(). Setting delta_pos manually within the control thread is not meaningful because there is no feedback for the control loop to provide PD control; this defeats the purpose of writing an independent control thread which is supposed to provide a corrective feedback control law.

If you manually set delta_pos to zero, the robot will continue to "stay at the current location" but it is unaware that the "current location now" has changed wrt "current location in previous step" because it moved slightly (due to things like uncompensated dynamics, inertia of motion, low gains, etc.). If you only want to control one direction, the best way is to give fixed position values for goal_pos for the directions you want and increment the other value for the direction you want the robot to move. This way the control thread has proper feedback to correct the errors that come up when moving. You can also try setting different controller gains for the motion, but I suspect that won't fully solve the problem on its own. You should definitely make sure that the control loop has feedback on the errors for providing corrective commands. Hope this helps.

-- Saif

[siweiyong]

@justagist Many thanks for your advice, it is really useful. By the way, is it possible to implement the hybrid motion-force controller based on the interface? Or direct contact force controller based on the interface? Many thanks.

[justagist]

Yes, it is possible to implement direct force control as well as hybrid control using the interface. There are several ways to do it and it is up to you to decide. If you want to do hybrid force-motion control in orthogonal directions, the basic idea is to compute the end-effector force (wrench actually) required to produce the motion, and the wrench required for providing the force control, then add them together to get the total end-effector wrench. You can then compute the joint commands (torques) using this as I have done in the example code you mentioned. The important thing is to make sure that you are doing force control and motion control in orthogonal directions! If you only want to do force control, you do the same as above but ignore the motion control part. A very simplified example of hybrid force-motion control is in this snippet: https://gist.github.com/justagist/3530cd05fbc17a6b1ce8832f8f5e2f8c#file-hybrid_force_motion_control-py. NOTE: this code cannot directly be used with the simulator, and is just a reduced and edited snippet from a different controller class I implemented for other purposes.

[siweiyong]

@justagist Thank you very much!

[siweiyong]

@justagist Hello Saif, thank you for your assistance using the library.

I have a question on the set_joint_torques() controller. Is the controller sampling 1kHz? I tried to use the torque controller, but the transient state (position and velocity) is not good enough. For example, I only track a position trajectory in the X direction, but the robot will oscillation in Z direction in low frequency or the final tracking error is not zero. But finally, the error in Z direction will be zero or a small value (1-2 cm error). In an ideal situation, the tracking error in Z and Y should be zero in the final state, and the robot would not oscillate during the tracking process.

I tried to do the same task by using the torque controller provided by libfranka, it seems the control process is smooth and steady. The transition performance and the steady performance is better than the set_joint_torques().

I tried to tune the stiffness gain and the damping gain for the set_joint_torques(), it is not good as the joint controller. I am wondering if the control frequency is 1000Hz or the parameter of my controller is bad. Did you test the controller on the real robot? If the problem is the parameters of the controller, could you please share the parameters of the controller for the real robot? Thank you very much.

[justagist]

The sampling rate when using franka_ros_interface will definitely be lower than 1000Hz, and will not be as fast as using libfranka directly, as ROS brings a lot of overhead in the communication. As I explained to you in the email, if you can avoid using franka_ros entirely and write your controller directly using libfranka, that is going to be the best option. There will also be delays because of the full robot state update that is happening continuously when using franka_ros_interface. But having said that, I use franka_ros_interface for my controllers via the set_joint_torques method and they are usually able to provide smooth motion for trajectory tracking. You should make sure that the control loop is running in an independent thread (better still, as an independent process) for the best performance. The parameters of the controller depend on the task you are trying to do and on the trajectory, etc. In my case, I usually have a controller loop (with set_joint_torques) running independently in a separate process (or computer) at a higher frequency, and send targets for the controller from another process at a lower frequency. Even then, it is unlikely that you can get your controller loop rate to match what you would get when directly using libfranka, so the controller parameters will have to be different to get similar performance. The parameters would also depend very much on how much frequency your higher-level controller loop can achieve reliably. It is much easier to tune your controllers for performance if you directly use libfranka and avoid using ROS.

[siweiyong]

Thank you very much for the suggestions. I will continue to tune the parameters of the controller. Many thanks again.