arm.jaw.servo_jp and arm.jaw.open() not working as expected

[Borgioli]

We would like to move the arm and the jaws simultaneously. We first created two separate codes to each move the arm and the jaws. There were no problems when running each code one at a time. The arm was controlled by the arm.servo_jp() and the jaw using the arm.jaw.open() function (so basically move_jp()). However, the jaws started to move very slowly when we ran the codes together. At every iteration, the jaw moves a fraction of the desired angle (we did not use the .wait() because we would like to move it in parallel). To solve this issue, we tried to run both the arm and jaw servo_jp functions but whenever it executed the jaw's servo_jp, the console shutdowns with PID tracking error for different joints of the arm (we tried different angles and durations).

[adeguet1]

Which version of the code are you using? I believe I do something similar in a Python test script and it seems to be working on my end: https://github.com/jhu-dvrk/dvrk-ros/blob/devel/dvrk_python/scripts/dvrk_psm_test.py

If your code is a python script, can you attach it?

[Borgioli]

We are using the current version (2.1.0), and we were referring to the sample code you posted. This is the python code used for moving the jaws.

start_angle = math.radians(np.copy(hand_app.psm_app.get_jaw_jp()))
goal = math.radians(hand_app.angle)
amplitude = start_angle-goal
duration = 1  
samples = int(duration / expected_interval)
for i in range(samples):
    goal = start_angle + amplitude * (math.cos(i * math.radians(360.0) / samples) - 1.0)
    hand_app.psm_app.arm.jaw.servo_jp(np.array(goal))
    rospy.sleep(expected_interval)

where hand_app.psm_app.get_jaw_jp() gets the current jaw jp and hand_app.angle is our desired angle.

This one was used to control the arm:

def run_servo_jp(self, goal, duration=5):
        initial_joint_position = np.copy(self.get_jp())
        samples = duration / self.expected_interval
        amplitude = (goal-initial_joint_position)/samples
        for i in range(int(samples)):
            cur_goal = initial_joint_position + i*amplitude
            self.arm.servo_jp(cur_goal)
            rospy.sleep(self.expected_interval)

Similarly, get_jp() gives the 6 joint angles of the PSM.

[adeguet1]

I had to refresh my mind and looked at the code. The current code doesn't support mixing PSM/servo_jp with PSM/jaw/move_jp. But you should be able to use PSM/servo_cp or PSM/servo_jp along with PSM/jaw/servo_jp. This is done in the dvrk_psm_test.py and in the dvrk_bag_replay.py. There is a small patch since 2.1 release but I don't think it will help with your issue so it's not worth using the development branches at that point.

For all servo commands, you need to send very small changes since there is no trajectory generation in the controller. Ideally you should send setpoints to the servo topics somewhere between 100Hz and 1KHz. One way to debug setpoints is to use plotjuggler (`rosrun plotjuggler plotjuggler) and plot your setpoint_jp and servo_jp. This should allow you to see the delta in position over time.

[Borgioli]

Using both the dvrk python package arm.jaw.servo_jp and the rostopic /PSM2/jaw/servo_jp produce the same results where once the first servo_jp is applied, the console shutdowns immediately. We recorded the /PSM2/jaw/setpoint_js instead since the rosbag was not able to record the position of /PSM2/jaw/servo_jp. However, here is a screenshot of a sequence of the published servo_jp and a plot showing what happens during the process (the servo_jp starts around 6.45).

Thank you!

[adeguet1]

I can't figure out what causes the issue you're running into. I think we should move this conversation to email or even zoom. Please email me so we can figure out how to proceed.

[Borgioli]

Thank you very much! I emailed you yesterday. I'm just letting you know here to make sure the email doesn't end up in the spam folder!

[adeguet1]

You are correct. Based on our Zoom call, it seems it's fixed in release 2.2.