Reset trajectory reference closer to the command loop

roboticslab-uc3m · Jan 17, 2023 · eb2a507 · eb2a507
1 parent eaea33e
commit eb2a507
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 19 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/external-pid/IControlModeRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/external-pid/IControlModeRawImpl.cpp
@@ -76,13 +76,30 @@ bool TechnosoftIposExternal::setControlModeRaw(int j, int mode)
         PdoConfiguration rpdo3conf;
         rpdo3conf.setTransmissionType(PdoTransmissionType::SYNCHRONOUS_CYCLIC);
 
-        return can->driveStatus()->requestState(DriveState::OPERATION_ENABLED)
+        bool ret = can->driveStatus()->requestState(DriveState::OPERATION_ENABLED)
             && can->rpdo3()->configure(rpdo3conf.addMapping<std::int32_t>(0x201C))
             && can->sdo()->download<std::uint16_t>("External Reference Type", 1, 0x201D)
             && can->sdo()->download<std::int8_t>("Modes of Operation", -5, 0x6060)
             // configure new setpoint (4: enable ext. ref. torque mode), reset other mode-specific bits (5-6) and halt bit (8)
             && can->driveStatus()->controlword(can->driveStatus()->controlword().set(4).reset(5).reset(6).reset(8))
             && awaitControlMode(mode);
+
+        // the point of the following instructions is to refresh the position reference as close to the command loop (in
+        // synchronize()) as possible; without this, the motor may jolt right after the transition from idle to command mode
+
+        if (ret) // successfully updated `actualControlMode`
+        {
+            if (mode == VOCAB_CM_POSITION || mode == VOCAB_CM_VELOCITY)
+            {
+                trajectory.reset(internalUnitsToDegrees(lastEncoderRead->queryPosition()));
+            }
+            else if (mode == VOCAB_CM_POSITION_DIRECT)
+            {
+                commandBuffer.reset(internalUnitsToDegrees(lastEncoderRead->queryPosition()));
+            }
+        }
+
+        return ret;
     }
 }
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/external-pid/IPositionControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/external-pid/IPositionControlRawImpl.cpp
@@ -20,13 +20,8 @@ bool TechnosoftIposExternal::positionMoveRaw(int j, double ref)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION);
 
-    double initialPosition = trajectory.isActive()
-        ? trajectory.queryPosition()
-        // account for any minimal displacements that can occur between mode change and the first command
-        : internalUnitsToDegrees(lastEncoderRead->queryPosition());
-
     trajectory.setTargetPosition(yarp::os::SystemClock::nowSystem(),
-                                 initialPosition, trajectory.queryVelocity(),
+                                 trajectory.queryPosition(), trajectory.queryVelocity(),
                                  ref, refSpeed, refAcceleration);
 
     return true;
@@ -40,13 +35,8 @@ bool TechnosoftIposExternal::relativeMoveRaw(int j, double delta)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION);
 
-    double initialPosition = trajectory.isActive()
-        ? trajectory.queryPosition()
-        // account for any minimal displacements that can occur between mode change and the first command
-        : internalUnitsToDegrees(lastEncoderRead->queryPosition());
-
     trajectory.setTargetPosition(yarp::os::SystemClock::nowSystem(),
-                                 initialPosition, trajectory.queryVelocity(),
+                                 trajectory.queryPosition(), trajectory.queryVelocity(),
                                  trajectory.queryPosition() + delta, refSpeed, refAcceleration);
 
     return true;

diff --git a/libraries/YarpPlugins/TechnosoftIpos/external-pid/IVelocityControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/external-pid/IVelocityControlRawImpl.cpp
@@ -29,13 +29,8 @@ bool TechnosoftIposExternal::velocityMoveRaw(int j, double sp)
         sp = std::clamp(sp, -maxVel, maxVel);
     }
 
-    double initialPosition = trajectory.isActive()
-        ? trajectory.queryPosition()
-        // account for any minimal displacements that can occur between mode change and the first command
-        : internalUnitsToDegrees(lastEncoderRead->queryPosition());
-
     trajectory.setTargetVelocity(yarp::os::SystemClock::nowSystem(),
-                                 initialPosition, trajectory.queryVelocity(),
+                                 trajectory.queryPosition(), trajectory.queryVelocity(),
                                  sp, refAcceleration);
 
     return true;