From 94d202013e250b8840b06fe36a7c055392c2eb3d Mon Sep 17 00:00:00 2001 From: Vincent Hui Date: Mon, 29 Apr 2019 17:33:10 +0800 Subject: [PATCH] reverse mtsPID: smoother velocities when in simulation mode commit e0aab8fe101f820ba02641bf8ea6f643cd6ecb67 introduced a bug that makes joint velocities are 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 3. 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 --- components/code/mtsPID.cpp | 41 +++++++++++--------------------------- 1 file changed, 12 insertions(+), 29 deletions(-) diff --git a/components/code/mtsPID.cpp b/components/code/mtsPID.cpp index 0b0c04f..a855c8b 100644 --- a/components/code/mtsPID.cpp +++ b/components/code/mtsPID.cpp @@ -864,42 +864,25 @@ void mtsPID::GetIOData(const bool computeVelocity) if (computeVelocity) { // or compute an estimate from position double dt = mPositionMeasure.Timestamp() - mPositionMeasurePrevious.Timestamp(); - if (!mIsSimulated) { - if (dt > 0) { - vctDoubleVec::const_iterator currentPosition = mPositionMeasure.Position().begin(); - vctDoubleVec::const_iterator previousPosition = mPositionMeasurePrevious.Position().begin(); - vctDoubleVec::iterator velocity; - const vctDoubleVec::iterator end = mVelocityMeasure.Velocity().end(); - for (velocity = mVelocityMeasure.Velocity().begin(); - velocity != end; - ++velocity) { - *velocity = (*currentPosition - *previousPosition) / dt; - } + if (dt > 0) { + vctDoubleVec::const_iterator currentPosition = mPositionMeasure.Position().begin(); + vctDoubleVec::const_iterator previousPosition = mPositionMeasurePrevious.Position().begin(); + vctDoubleVec::iterator velocity; + const vctDoubleVec::iterator end = mVelocityMeasure.Velocity().end(); + for (velocity = mVelocityMeasure.Velocity().begin(); + velocity != end; + ++velocity) { + *velocity = (*currentPosition - *previousPosition) / dt; } } else { - // for simulation - if (dt > 0) { - vctDoubleVec::const_iterator currentPosition = mPositionMeasure.Position().begin(); - vctDoubleVec::const_iterator previousPosition = mPositionMeasurePrevious.Position().begin(); - vctDoubleVec::iterator velocity; - const vctDoubleVec::iterator end = mVelocityMeasure.Velocity().end(); - for (velocity = mVelocityMeasure.Velocity().begin(); - velocity != end; - ++velocity) { - if (*currentPosition != *previousPosition) { - *velocity = (*currentPosition - *previousPosition) / dt; - } - } - } - + // dt is useless set to zero to be safe + mVelocityMeasure.Velocity().SetAll(0.0); } } else { // user requested to not compute velocities, likely // because previousPosition can't be trusted (e.g. after // coupling change) - if (!mIsSimulated) { - mVelocityMeasure.Velocity().SetAll(0.0); - } + mVelocityMeasure.Velocity().SetAll(0.0); } }