PSMs don't reach commanded set-points

[kschwan]

Hi @adnanmunawar,
I'm revisiting Task 2 (needle grasping and driving) as we have some students wanting to use the simulated environment in a project. I'm experiencing some issues however:

It doesn't seem like the PSM's are able to reach the commanded joint space set-points. Are they not supposed to, can it be improved, or is it an error on my part?

I attach an example where I command the robot to move to a Needle/Grasp frame. The joint space set-point is computed through surgical_robotics_challenge.kinematics.psmIK.compute_IK and published on the /CRTK/psm2/servo_jp topic (interpolating a trajectory in joint space).

The images below show the simulator view, and the commanded Needle/Grasp frame and current /CRTK/psm2/measured_cp pose.

psm2.txt has rostopic output corresponding to the state shown in the images.

I interface with the simulator via launch_crtk_interface.py and I have set add_joint_errors=False.

I use 314a4ba and WPI-AIM/ambf@18637b4.

PS. I have the same issue using velocity control.

[JackHaoyingZhou]

Hi @adnanmunawar and @kschwan ,

I have also found out the same issue with the latest repository.

Last year I designed a customized recorder directly recording the self._ik_solution (psm_arm.py, line#167) therefore I manage to avoid this problem. This year, I attempt to use rosbag for recording and the problem starts to come out.

In addition to the description of @kschwan, I print out the joint positions directly from servo_jp and measured_jp simultaneously. You may check the following screenshot for the information. And we can see that all the joints have minor differences between servo_jp and measured_jp. Would you mind giving some hints about how to solve this problem? Thank you!

[adnanmunawar]

Hi Kim and Jack,

You guys are correct, there is indeed a steady state error in arm's joint controller loops. This is due to the default controller gains for each joint (https://github.com/collaborative-robotics/surgical_robotics_challenge/blob/master/ADF/psm1.yaml#L697), since we don't specify the PID gains, they default to (P: 10, I: 0. D:1). Once can optimize these gains to reduce the steady-state error and here is an example of how to manually set the gains (https://github.com/WPI-AIM/ambf/blob/ambf-2.0/ambf_models/descriptions/multi-bodies/robots/blender-kuka-lbr-med.yaml#L557). One can also use the Blender addon to set the gains before generating the ADF file as shown below:

Lastly, if you disable gravity by pressing the 2 key on the keyboard, the steady-state error would go to zero, but the other objects in the scene would also start floating around. We can add a per object gravity setting so that we can disable the gravity only for the links of the PSM arms and that way not have to worry about optimizing the controller gains for optimal positioning.

[kschwan]

Thanks Adnan, sounds good, I'll pass on the information!

[adnanmunawar]

Hi Guys,
I have implemented the setting of per object or per model gravity in AMBF. Please see the discussion here (WPI-AIM/ambf#199).

Please pull the latest version of ambf and build it. Then one can set zero gravity at the model level scope of the psm1, psm2, psm3 files and the position control should be accurate.

[adnanmunawar]

Please checkout the commit above. Don't forget to pull the latest ambf commit and build it before trying out the SRC.

[JackHaoyingZhou]

Hi Adnan,

Hope all is well!

Thank you for the update! Unfortunately, I have attempted the latest version with latest AMBF. It seems that the problem has not been resolved yet.

I also print out the joint positions directly from servo_jp and measured_jp simultaneously. You may check the following screenshot for the information. And we can see that all the joints still have minor differences between servo_jp and measured_jp.

It may involve fine tuning the PID parameters for accurate control. Or would you mind giving some other hints which may be helpful? Thank you!

[adnanmunawar]

Hi Jack,

Thanks. A fews things to make sure. Did you pull and rebuild AMBF? Then did you pull the latest version of SRC? When you load the SRC, you should see messages in the console saying something like "INFO! Setting OBJECT's gravity to ...". Finally, are you sure that the commanded joint positions are not resulting in collision of the PSMs with the environment? Finally, just to make sure, have you set the joint errors to 0.0?

[JackHaoyingZhou]

Hi Adnan,

Thank you for the instruction!

For most of the points, I am in the same page with you, except that didn't correctly source the latest built AMBF, therefore it doesn't run as expected. In the previous running, the message "INFO! Setting OBJECT's gravity to ..." didn't show up. After fixing this problem, I tested again, and the result seems good. It has very slight errors which can be ignored in most times:

One more point to bring for replaying: If somebody record the topic "/ambf/env/psm1(2)/baselink/State", when they want to replay, they may need to divide the third joint (the prismatic joint) value by 10 before feeding to psm.servo_jp() command. And sometimes the gripper command may be not reliable because of some obstacles or blocking in the simulation scene. They can consider using an offset if necessary.

[adnanmunawar]

Hi Jack, thanks for this info. I would suggest that if you are recording the state (joint space or Cartesian space) of the PSMs, you should record them from the measured_jp topic that is available after running the launch_crtk_interface.py script. This script also launches a subscriber called servo_jp (and very soon we will add `move_jp'). Recording and publishing via these topics (interfaces) will take care of the scaling issue.

[adnanmunawar]

Closing this as resolved