diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp index 5b5d9f5d10e928ef8ef35ee5a42cd78c4198fd91..3a31f21bba9a5f75d22d08a7b7e2e9d574db1f49 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp @@ -52,11 +52,19 @@ namespace armarx void NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController() { + const float velX = velocitySensor->velocityX; + const float velY = velocitySensor->velocityY; + const float velRot = velocitySensor->velocityRotation; + // init velocity ramp Eigen::VectorXf state(6); - state << velocitySensor->velocityX, velocitySensor->velocityY, 0, 0, 0, - velocitySensor->velocityRotation; + state << velX, velY, 0, 0, 0, velRot; ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All); + + getWriterControlStruct().velocityX = velX; + getWriterControlStruct().velocityY = velY; + getWriterControlStruct().velocityRotation = velRot; + writeControlStruct(); } void