3 #include <VirtualRobot/RobotNodeSet.h>
4 #include <VirtualRobot/math/Helpers.h>
17 #include <armarx/control/njoint_controller/platform/platform_follower_controller/aron/PlatformFollowerControllerConfig.aron.generated.h>
22 const std::string
NAME =
"PlatformFollowerController";
27 const armarx::NJointControllerConfigPtr& config,
31 ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
40 armarx::ControlModes::HolonomicPlatformVelocity)
43 <<
"The actuator " << robotUnit->getRobotPlatformName() <<
" has no control mode "
44 << armarx::ControlModes::HolonomicPlatformVelocity;
46 const Config configData = ::armarx::fromAron<arondto::Config, Config>(cfg->config);
52 const VirtualRobot::RobotNodeSetPtr rnsRight =
53 robot->getRobotNodeSet(configData.
params.nodeSetNameRight);
55 const VirtualRobot::RobotNodeSetPtr rnsLeft =
56 robot->getRobotNodeSet(configData.
params.nodeSetNameLeft);
58 tcpRight = rnsRight->getTCP();
59 tcpLeft = rnsLeft->getTCP();
73 stateBuffer.
getWriteBuffer().handPositionRight = tcpRight->getPositionInRootFrame();
74 stateBuffer.
getWriteBuffer().handPositionLeft = tcpLeft->getPositionInRootFrame();
95 const Ice::Current& iceCurrent)
101 auto updateConfig = ::armarx::fromAron<arondto::Config, Config>(dto);
108 if (not rtReady.load())
124 const double deltaT = (state.time - additionalTaskData.lastTime).toSecondsDouble();
125 additionalTaskData.lastTime = state.time;
130 const Eigen::Vector3f currentVel = state.currentVel;
131 const Eigen::Vector3f currentPositionRight = state.handPositionRight;
132 const Eigen::Vector3f currentPositionLeft = state.handPositionLeft;
133 const Eigen::Vector3f positionDeltaRight =
135 const Eigen::Vector3f positionDeltaLeft =
138 Eigen::Vector2f positionDelta = ((positionDeltaLeft + positionDeltaRight) / 2).head(2);
139 Eigen::Vector2f velocity = Eigen::Vector2f::Zero();
140 if (positionDelta.norm() >= params.minPosTolerance)
143 positionDelta - params.minPosTolerance * positionDelta / positionDelta.norm();
144 velocity = positionDelta * params.kPos - currentVel.head(2) * params.dPos;
146 const float Tstar = 1 / ((params.filterTimeConstant / deltaT) + 1);
147 additionalTaskData.filteredLinearVel =
148 Tstar * (velocity - additionalTaskData.filteredLinearVel) +
149 additionalTaskData.filteredLinearVel;
152 additionalTaskData.filteredLinearVel(0) = 0;
156 additionalTaskData.filteredLinearVel(1) = 0;
158 velocity = additionalTaskData.filteredLinearVel.norm() > params.maxPosVel
159 ? additionalTaskData.filteredLinearVel.normalized() * params.maxPosVel
160 : additionalTaskData.filteredLinearVel;
163 const Eigen::Vector3f currentDirection = state.direction();
164 const float cosAlpha = currentDirection.dot(initialState.
direction());
165 const Eigen::Vector3f directionDiff = currentDirection - initialState.
direction();
167 if (directionDiff(1) < 0)
169 alpha = -acos(cosAlpha);
173 alpha = acos(cosAlpha);
175 float angularVelocity;
176 if (fabs(alpha) < params.minAngleTolerance)
184 alpha -= params.minAngleTolerance;
188 alpha += params.minAngleTolerance;
190 angularVelocity = params.kAngle * alpha - params.dAngle * currentVel.z();
192 additionalTaskData.filteredAngularVel =
193 Tstar * (angularVelocity - additionalTaskData.filteredAngularVel) +
194 additionalTaskData.filteredAngularVel;
197 additionalTaskData.filteredAngularVel = 0;
199 angularVelocity =
std::min(additionalTaskData.filteredAngularVel, params.maxAngleVel);
205 onPublishData.target.platformVelocityTargets.x = velocity.x();
206 onPublishData.target.platformVelocityTargets.y = velocity.y();
207 onPublishData.target.platformVelocityTargets.yaw = angularVelocity;
209 onPublishData.position_error_left = positionDeltaLeft;
210 onPublishData.position_error_right = positionDeltaRight;
244 debugObservers->setDebugChannel(
NAME, datafields);
260 armarx::eManagedIceObjectStarted);
261 ARMARX_IMPORTANT <<
"Create a new thread alone PlatformTrajectory controller";
262 while (
getState() == armarx::eManagedIceObjectStarted)
269 c.waitForCycleDuration();
273 ARMARX_INFO <<
"PlatformTrajectoryVelocityController::onInitNJointController";