diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp index 4e9d7fefe2579dec0297037806cf4ea531c9e32f..2e5642d3fb78c95afd81644dec24d4c13f313ff4 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp @@ -50,14 +50,13 @@ namespace armarx void NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController() { - const float velX = velocitySensor->velocityX; - const float velY = velocitySensor->velocityY; - const float velRot = velocitySensor->velocityRotation; + activationVelocity(0) = velocitySensor->velocityX; + activationVelocity(1) = velocitySensor->velocityY; + activationVelocity(2) = velocitySensor->velocityRotation; + activationTime = IceUtil::Time::now(); // init velocity ramp - ramp.init(Eigen::Vector3f(velX, velY, velRot)); - - setVelocites(velX, velY, velRot); + ramp.init(activationVelocity); } void @@ -79,11 +78,19 @@ namespace armarx } else { - Eigen::Vector3f x(rtGetControlStruct().velocityX, - rtGetControlStruct().velocityY, - rtGetControlStruct().velocityRotation); + Eigen::Vector3f result; + if (activationTime > rtGetControlStruct().commandTimestamp) + { + result = ramp.update(activationVelocity, timeSinceLastIteration.toSecondsDouble()); + } + else + { + Eigen::Vector3f x(rtGetControlStruct().velocityX, + rtGetControlStruct().velocityY, + rtGetControlStruct().velocityRotation); + result = ramp.update(x, timeSinceLastIteration.toSecondsDouble()); + } - Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble()); target->velocityX = result(0); target->velocityY = result(1); target->velocityRotation = result(2); diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h index 9d2adec8f816e52bcd71179cbe322f9eb88317a0..8cd957b50d7911524c6e5d68a91ad4359213f145 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h @@ -93,6 +93,9 @@ namespace armarx protected: Cartesian2DimVelocityRamp ramp; + Eigen::Vector3f activationVelocity; + IceUtil::Time activationTime; + ControlTargetHolonomicPlatformVelocity* target; const SensorValueHolonomicPlatformVelocity* velocitySensor; };