[BUG] setNullSpacePosture() does not take effect at the beginning of the movement

[acf986]

Version information

• RDK: v0.9
• Robot software: unknown (we are using the hotfix version for this issue)
• OS: Ubuntu 20.04

Describe the bug What I am trying to do:

• Bring the robot arm to a desired posture through joint movement.
• Set the desired nullspace posture to the current joint values.
• Start to follow a cartesian trajectory with a NRT_CARTESIAN_MOTION_FORCE controller.

What is the issue:

• The robot arm twitches violently at the beginning of the NRT_CARTESIAN_MOTION_FORCE move.

The possible reason:

• It seems, at the first moment the robot arm starts to move in the NRT_CARTESIAN_MOTION_FORCE mode, the nullspace posture setting is not taking effect, and the robot is still using the default null space posture, therefore, there is a sudden twitches of the robot arm.

Steps to reproduce

1. Bring the robot arm to a different nullspace posture that is largely different from the default one. This can be done either manually or through MoveJ primitive.
2. Switch the robot to NRT_CARTESIAN_MOTION_FORCE mode.
3. Use getRobotStates() to get the current robot state.
4. Use setNullSpacePosture() to set the NullSpacePosture to the current joint angles.
5. Start to move the robot arm by calling sendCartesianMotionForce().
6. The arm will twitch once at the first moment of moving.

Expected behavior The robot arm should move smoothly without twitch, and also with the updated null space posture.

Screenshots

Additional context

[pzhu-flexiv]

@acf986 This is an expected behavior. According to the warning note of setNullSpacePosture(), the preferred joint position will be reset to nominal values when RT/NRT_CARTESIAN_MOTION_FORCE mode is re-entered.

[acf986]

@pzhu-flexiv Thanks for your answer. However, after calling setNullSpacePosture(), we never leave the NRT_CARTESIAN_MOTION_FORCE mode, so there should be no reset?

More specifically, we call setNullSpacePosture() after the robot entered the NRT_CARTESIAN_MOTION_FORCE mode, and before the first call to sendCartesianMotionForce().

[pzhu-flexiv]

@acf986 The reset happens when you enter NRT_CARTESIAN_MOTION_FORCE from other modes.

[acf986]

@pzhu-flexiv Thanks for your response! But after entered NRT_CARTESIAN_MOTION_FORCE mode, we immediately called setNullSpacePosture() before sending out any motion/force command. Won't this set the null space posture to the desired value instead of the default one?

[pzhu-flexiv]

@acf986 Currently no, the reset will force to the default value before taking the newly set value. However, after a second thought, we think it's better to not force the reset given use cases like yours. The change will be implemented in v0.9.2, please wait for the update.

[acf986]

@pzhu-flexiv Thanks very much! In that case, let me just give more details. What we hope the robot will do:

1. Bring the robot arm to a desired posture through joint movement.
2. Set the desired nullspace posture to the current joint values.
3. Start to follow a cartesian trajectory with a NRT_CARTESIAN_MOTION_FORCE controller.

Currently, for step 2 and 3, if we use the built in primitive (like compliance move), it works without any issue. The robot will successfully update its null space posture before executing the primitive's motion. We just hope the NRT_CARTESIAN_MOTION_FORCE controller could achieve the same.