Jaw setpoint is (re)set going from goal to direct control mode

[kschwan]

When Cartesian space setpoints are set with SetPositionCartesian, the jaw opening angle setpoint is (re-)set when going from goal to direct control mode, causing a PID error or the grasper to close very suddenly.

The problem is illustrated in the below script:

#!/usr/bin/env python

import dvrk
import numpy as np
import PyKDL
import rospy

rospy.init_node('psm_move_interp_jaw_test', disable_signals=True)
psm = dvrk.psm('PSM1')

while psm.get_arm_current_state() != 'READY':
    rospy.logwarn("PSM state != 'READY'")
    rospy.sleep(1)

# Move to an initial position
q_home = np.zeros_like(psm.get_current_joint_position())
q_home[2] = 0.12
psm.move_joint(q_home)
psm.move_jaw(np.pi/4)

# (jaw is now open)
rospy.sleep(1)

# Move setpoint a tiny bit in the z direction,
# switching to direct control mode.
# - the jaw closes with interpolate=False
# - but stays open interpolate=True
f = psm.get_current_position()
f.p[2] += 1e-6
# psm.move(f, interpolate=True)
psm.move(f, interpolate=False)