public UserDesiredHandstepProvider(
FullHumanoidRobotModel fullRobotModel,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
this.handstepHelper = new HandstepHelper(fullRobotModel);
userHandstepTakeIt.set(false);
userHandstepNormal.set(-1.0, 0.0, 0.0);
userHandstepRobotSide.set(RobotSide.LEFT);
userDesiredHandstepCoordinateSystem =
new YoGraphicCoordinateSystem("userHandstepViz", "", parentRegistry, 0.3);
VariableChangedListener listener =
new VariableChangedListener() {
public void variableChanged(YoVariable<?> v) {
Vector3d position = userHandstepPosition.getVector3dCopy();
Vector3d surfaceNormal = userHandstepNormal.getVector3dCopy();
double rotationAngleAboutNormal = userHandstepRotationAboutNormal.getDoubleValue();
userDesiredHandstepCoordinateSystem.setTransformToWorld(
HandstepHelper.computeHandstepTransform(
false, position, surfaceNormal, rotationAngleAboutNormal));
}
};
yoGraphicsListRegistry.registerYoGraphic(
"UserDesiredHandstep", userDesiredHandstepCoordinateSystem);
userHandstepNormal.attachVariableChangedListener(listener);
userHandstepRotationAboutNormal.addVariableChangedListener(listener);
userHandstepPosition.attachVariableChangedListener(listener);
parentRegistry.addChild(registry);
}
