reverse mtsPID: smoother velocities when in simulation mode

[vincent-hui]

commit aeb68cf introduced a bug that makes joint velocities be not zero even when joints are stationary.
To reproduce the issue

1. run dVRK console with a MTMR configuration file
2. press "home" button on dVRK console

rostopic pub /dvrk/MTMR/set_position_goal_cartesian geometry_msgs/Pose "position:
x: -0.02814
y: -0.06600
z: 0.40150
orientation:
x: 0.5
y: 0.5
z: 0.5
w: -0.5" -1
4. press "power off" button on dVRK console
5. press "home" button on dVRK console
6. you can see some joint efforts are not zeros because some joint velocities are not zeros.

[adeguet1]

@vincent-hui , can you confirm that this is in KIN simulated mode? When using a real robot all velocities are estimated on the FPGA and the PID component should not modify these. To be honest, in kinematic simulated mode, since the robot moves instantly to the desired position, efforts don't really mean anything.

[adeguet1]

To note, I pushed some changes on the devel branch to provide a slightly better velocity estimation using the setpoint/commands sent to the PID in kinematic simulation mode. See 8cd3530

[vincent-hui]

At line#892 of mtsPID.cpp

    if (*currentPosition != *previousPosition) {
        *velocity = (*currentPosition - *previousPosition) / dt;
    }

In sequence of "home" -> move joint by /dvrk/MTMR/set_position_goal_joint -> "power off" -> "home".
After second "home" is cliked , *currentPosition is set to 0 but *previousPosition is not set to *currentPosition before GetIOData(true) is called. Therefore *velocity will be updated
at line#897 *velocity = (*currentPosition - *previousPosition) / dt;.
After GetIOData(true) is called, *previousPosition is set to *currentPosition
at line#587 mPositionMeasurePrevious = mPositionMeasure;.
Therefore *velocity will not be updated.
That leads to joint velocities and efforts being not zero even when joints are stationary after second "home" is clicked.

void mtsPID::Run(void)
{
    ProcessQueuedEvents();
    ProcessQueuedCommands();

    // get data from IO if not in simulated mode
    GetIOData(true); // compute velocity if needed
......
    mPositionMeasurePrevious = mPositionMeasure;
......
}

[adeguet1]

Hi Vincent,

I'm trying to understand the ultimate goal for the "kinematic" simulation mode. My intentions are:

• Provide a simple way to test position control without an actual arm
• Since we have some desired positions sent by user, we might be able to approximate velocities (but to be honest, I don't know anyone using these). One of the issues is that we can only compute velocities when the user sends new target positions.
• Efforts (both desired and measured) are useless. The current implementation used to compute them and keep them around but the right thing to do in my mind is to set them to zero since they are meaningless (this is what I did in the last devel commit).

Can you provide an example of use for the velocities and efforts in simulation mode? I get the feeling that maybe the solution is to keep the PID component as is but create a different IO component (e.g. for dynamic simulation) that will receive the desired efforts and provide a simulated measured position/velocity/effort.

[vincent-hui]

Hi Anton,

I think efforts are not useless. We can observe joint efforts produced by gravity compensation without an actual arm too in the "kinematic" simulation mode. We can check whether the joint efforts computed in C++ are equivalent to the joint efforts computed in Matlab easily for any joint configuration produced by gravity compensation by looking at Desired effort or Current effort on PID widgets of dVRK console as the attached image shown

after enabling gravity compensation and sending a zero wrench. I think we should keep this feature because it is beneficial to the dVRK community.

I don't use joint velocities in simulation mode. In position mode, I just want to make sure joint velocities are zeros when joints are stationary so that zero joint efforts can be produced and users will not get confused by non zero joint efforts.

[adeguet1]

Thank you for the clarification, I didn't see any value using the effort mode in simulation but your application makes sense. The current PID code supports two alternate modes for each joint. A single joint can be driven in position OR effort. For the MTMs, when we set the efforts for the gravity compensation, all the joints are driven in effort mode. So I think the specs you would like to have in simulation mode:

• if the joint is driven in position:
  • position is the position sent by user
  • velocity can be approximated based on user positions over time
  • effort is set to zero
• if the joint is driven in effort:
  • position remains unchanged
  • velocity is set to zero (not moving)
  • effort is the effort sent by user

In both cases, I assume we should set the measured joint state based on the desired one.

Does this make sense? If we agree, I'll rewrite the code using two Run methods, i.e. RunPhysical and RunSimulated. The main Run method can then call one or the other based on the mIsSimulated flag.

[vincent-hui]

The specs you wrote is very similar to what I think.

In position mode
I think joint effort should not be set to zero explicitly. We just need to make sure velocities of stationary joints are zeros. When some joints are moving, non zero joint effort is not an issue because joint efforts will become zeros after joints stop moving.
The issue in the latest PID code on devel branch is that mPositionMeasurePrevious is not reset to zero when "home" button is clicked. mPositionMeasurePrevious should be set to zero when "home" button is clicked so that velocity remains zero and produces zero effort.

In effort mode
I agree with you

I propose two changes.

1. reset mPositionMeasurePrevious to zero when "home" button is clicked.
2. velocity is set to zero if the joint is in effort mode.

You can decide how to implement these changes.
Do you agree with me?

[adeguet1]

I've been looking into different ways to get a reasonable velocity estimation when in simulation mode and I think the best way is to use the data sent by the "user", i.e. the arm class. When nothing is sent for a bit (20 ms), velocity goes back to zero. I also patched the code to have zero velocity if effort mode is used. @vincent-hui , can you test cdd8be4? Unfortunately, this also requires the latest sawIntuitiveResearchKit devel branch.

[vincent-hui]

Thank you for your patch, I tested cdd8be4 . It works well.