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); }