@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 300000)
public void testGetYoVariableRegistry() {
YoVariableRegistry registry = yoVariable.getYoVariableRegistry();
assertNotNull(registry);
assertEquals(registry, this.registry);
assertEquals(registry.getVariable(yoVariable.getName()), yoVariable);
}
@Before
public void setUp() {
YoVariableRegistry robotRegistry = new YoVariableRegistry("robot");
registry = new YoVariableRegistry("testRegistry");
robotRegistry.addChild(registry);
yoVariable = new DoubleYoVariable("variableOne", registry);
variableChangedListeners = new ArrayList<TestVariableChangedListener>();
}
public CentroidalMomentumHandler(
RigidBody rootBody, ReferenceFrame centerOfMassFrame, YoVariableRegistry parentRegistry) {
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootBody, centerOfMassFrame);
this.previousCentroidalMomentumMatrix =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix =
new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
[previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(
yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
for (InverseDynamicsJoint joint : jointsInOrder) {
TIntArrayList listToPackIndices = new TIntArrayList();
ScrewTools.computeIndexForJoint(jointsInOrder, listToPackIndices, joint);
int[] indices = listToPackIndices.toArray();
columnsForJoints.put(joint, indices);
}
centroidalMomentumRate = new SpatialForceVector(centerOfMassFrame);
this.centerOfMassFrame = centerOfMassFrame;
parentRegistry.addChild(registry);
double robotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
this.centroidalMomentumRateTermCalculator =
new CentroidalMomentumRateTermCalculator(rootBody, centerOfMassFrame, v, robotMass);
}
public QuadrupedXGaitStepStream(
QuadrupedPlanarVelocityInputProvider planarVelocityProvider,
QuadrupedXGaitSettingsInputProvider xGaitSettingsProvider,
QuadrupedReferenceFrames referenceFrames,
double controlDT,
DoubleYoVariable timestamp,
YoVariableRegistry parentRegistry) {
this.planarVelocityProvider = planarVelocityProvider;
this.xGaitSettingsProvider = xGaitSettingsProvider;
this.xGaitSettings = new QuadrupedXGaitSettings();
this.xGaitStepPlanner = new QuadrupedXGaitPlanner();
this.xGaitPreviewSteps = new ArrayList<>(NUMBER_OF_PREVIEW_STEPS);
this.xGaitCurrentSteps = new EndDependentList<>();
for (int i = 0; i < NUMBER_OF_PREVIEW_STEPS; i++) {
xGaitPreviewSteps.add(new QuadrupedTimedStep());
}
for (RobotEnd robotEnd : RobotEnd.values) {
xGaitCurrentSteps.set(robotEnd, new QuadrupedTimedStep());
}
this.supportCentroid = new FramePoint();
this.supportFrame = referenceFrames.getCenterOfFeetZUpFrameAveragingLowestZHeightsAcrossEnds();
this.bodyZUpFrame = referenceFrames.getBodyZUpFrame();
this.worldFrame = ReferenceFrame.getWorldFrame();
this.controlDT = controlDT;
this.timestamp = timestamp;
this.bodyOrientation = new FrameOrientation();
this.stepSequence =
new PreallocatedList<>(QuadrupedTimedStep.class, NUMBER_OF_PREVIEW_STEPS + 2);
if (parentRegistry != null) {
parentRegistry.addChild(registry);
}
}
public TaskspaceHandControlState(
String namePrefix,
HandControlState stateEnum,
MomentumBasedController momentumBasedController,
int jacobianId,
RigidBody base,
RigidBody endEffector,
YoVariableRegistry parentRegistry) {
super(stateEnum);
name =
namePrefix
+ FormattingTools.underscoredToCamelCase(this.getStateEnum().toString(), true)
+ "State";
registry = new YoVariableRegistry(name);
taskspaceConstraintData.set(base, endEffector);
this.momentumBasedController = momentumBasedController;
this.jacobianId = jacobianId;
this.base = base;
this.endEffector = endEffector;
parentRegistry.addChild(registry);
}
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);
}
/**
* Quadruped version, assumes flat ground
*
* @param footName name for yovariable registry and yovariable names
* @param quadrupedFeet all the feet
* @param switchZThreshold difference in z between the lowest foot and the foot inquestion before
* assuming foot is in contact
* @param totalRobotWeight
* @param quadrant the foot in question
* @param parentRegistry
*/
public KinematicsBasedFootSwitch(
String footName,
QuadrantDependentList<? extends ContactablePlaneBody> quadrupedFeet,
double switchZThreshold,
double totalRobotWeight,
RobotQuadrant quadrant,
YoVariableRegistry parentRegistry) {
registry = new YoVariableRegistry(footName + getClass().getSimpleName());
foot = quadrupedFeet.get(quadrant);
ContactablePlaneBody acrossBodyFrontFoot =
quadrupedFeet.get(quadrant.getAcrossBodyFrontQuadrant());
ContactablePlaneBody acrossBodyHindFoot =
quadrupedFeet.get(quadrant.getAcrossBodyHindQuadrant());
ContactablePlaneBody sameSideFoot = quadrupedFeet.get(quadrant.getSameSideQuadrant());
otherFeet = new ContactablePlaneBody[] {acrossBodyFrontFoot, acrossBodyHindFoot, sameSideFoot};
this.totalRobotWeight = totalRobotWeight;
hitGround = new BooleanYoVariable(footName + "hitGround", registry);
fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
soleZ = new DoubleYoVariable(footName + "soleZ", registry);
ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
this.switchZThreshold.set(switchZThreshold);
yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);
parentRegistry.addChild(registry);
}
public ForcePerturbance(
ForcePerturbable forcePerturbable,
double magnitude,
double duration,
double ballVelocityMagnitudeForViz,
YoVariableRegistry parentRegistry) {
this.forcePerturbable = forcePerturbable;
this.disturbanceMagnitude.set(magnitude);
this.disturbanceDuration.set(duration);
this.ballVelocityMagnitude = ballVelocityMagnitudeForViz;
if (parentRegistry != null) parentRegistry.addChild(registry);
}
public DiagnosticTaskExecutor(
String namePrefix, DoubleYoVariable yoTime, YoVariableRegistry parentRegistry) {
parentRegistry.addChild(registry);
currentTaskIndex = new IntegerYoVariable(namePrefix + "CurrentTaskIndex", registry);
numberOfTasksRemaining = new IntegerYoVariable(namePrefix + "TasksRemaining", registry);
isDone = new BooleanYoVariable(namePrefix + "IsDone", registry);
hasAborted = new BooleanYoVariable(namePrefix + "HasAborted", registry);
timeInCurrentTask = new DoubleYoVariable(namePrefix + "TimeInCurrentTask", registry);
switchTime = new DoubleYoVariable(namePrefix + "SwitchTime", registry);
this.yoTime = yoTime;
clear();
}
@Override
public void onNewMessage(std_msgs.Float32MultiArray msg) {
if (msg._TYPE.equals(this.getMessageType())) {
java.util.List<std_msgs.MultiArrayDimension> dims = msg.getLayout().getDim();
float[] data = msg.getData();
if (dims.size() != 2)
System.out.println("ERROR: Multisense message data is not a 2d array.");
if (!(dims.get(0).getSize() == 1 && dims.get(1).getSize() == 2))
System.out.println("ERROR: Multisense message data is not a 1x2 vector array.");
magnitudeMeters = data[dims.get(0).getSize() * 0 + dims.get(1).getStride() * 0];
angleRadians = data[dims.get(0).getSize() * 0 + dims.get(1).getStride() * 1];
DoubleYoVariable vh = (DoubleYoVariable) registry.getVariable("multisenseHeading");
DoubleYoVariable vm = (DoubleYoVariable) registry.getVariable("multisenseMagnitude");
vh.setValueFromDouble(angleRadians);
vm.setValueFromDouble(magnitudeMeters);
} else {
System.out.println("ERROR: Invalid message. Expecting " + this.getMessageType());
}
}
public MomentumSolver3(
SixDoFJoint rootJoint,
RigidBody elevator,
ReferenceFrame centerOfMassFrame,
TwistCalculator twistCalculator,
LinearSolver<DenseMatrix64F> jacobianSolver,
double controlDT,
YoVariableRegistry parentRegistry) {
this.rootJoint = rootJoint;
this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootJoint.getSuccessor());
this.motionConstraintHandler =
new MotionConstraintHandler("solver3", jointsInOrder, twistCalculator, null, registry);
this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
this.previousCentroidalMomentumMatrix =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
this.centroidalMomentumMatrixDerivative =
new DenseMatrix64F(
centroidalMomentumMatrix.getMatrix().getNumRows(),
centroidalMomentumMatrix.getMatrix().getNumCols());
yoPreviousCentroidalMomentumMatrix =
new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
[previousCentroidalMomentumMatrix.getNumCols()];
MatrixYoVariableConversionTools.populateYoVariables(
yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);
this.controlDT = controlDT;
nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
this.b = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);
// nullspaceMotionConstraintEnforcer = new
// EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
equalityConstraintEnforcer =
new EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
solver = LinearSolverFactory.pseudoInverse(true);
parentRegistry.addChild(registry);
reset();
}
private WandererSingleThreadedController(
WandererRobotModel robotModel,
PriorityParameters priorityParameters,
RobotVisualizer visualizer,
WandererController wandererController,
YoVariableRegistry registry) {
super(priorityParameters);
this.state = new WandererState(0.001, registry);
this.reader = new UDPAcsellStateReader(state);
this.visualizer = visualizer;
this.command = new WandererCommand(registry);
this.controller = wandererController;
this.outputWriter = new UDPAcsellOutputWriter(command);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
rootJoint = fullRobotModel.getRootJoint();
for (WandererJoint joint : WandererJoint.values) {
OneDoFJoint oneDoFJoint = fullRobotModel.getOneDoFJointByName(joint.getSdfName());
if (oneDoFJoint == null) {
throw new RuntimeException("Unknown joint: " + joint.getSdfName());
}
jointMap.put(joint, oneDoFJoint);
}
this.rawSensors = new RawJointSensorDataHolderMap(fullRobotModel);
this.controller.setFullRobotModel(fullRobotModel);
if (controller.getYoVariableRegistry() != null) {
registry.addChild(controller.getYoVariableRegistry());
}
if (visualizer != null) {
visualizer.setMainRegistry(registry, fullRobotModel, null);
}
outputWriter.connect(new WandererNetworkParameters());
}
public AxisAngleOrientationController(
String prefix,
ReferenceFrame bodyFrame,
double dt,
YoOrientationPIDGainsInterface gains,
YoVariableRegistry parentRegistry) {
this.dt = dt;
this.bodyFrame = bodyFrame;
registry = new YoVariableRegistry(prefix + getClass().getSimpleName());
if (gains == null) gains = new YoAxisAngleOrientationGains(prefix, registry);
this.gains = gains;
proportionalGainMatrix = gains.createProportionalGainMatrix();
derivativeGainMatrix = gains.createDerivativeGainMatrix();
integralGainMatrix = gains.createIntegralGainMatrix();
rotationErrorInBody = new YoFrameVector(prefix + "RotationErrorInBody", bodyFrame, registry);
rotationErrorCumulated =
new YoFrameVector(prefix + "RotationErrorCumulated", bodyFrame, registry);
velocityError = new YoFrameVector(prefix + "AngularVelocityError", bodyFrame, registry);
proportionalTerm = new FrameVector(bodyFrame);
derivativeTerm = new FrameVector(bodyFrame);
integralTerm = new FrameVector(bodyFrame);
feedbackAngularAction =
new YoFrameVector(prefix + "FeedbackAngularAction", bodyFrame, registry);
rateLimitedFeedbackAngularAction =
RateLimitedYoFrameVector.createRateLimitedYoFrameVector(
prefix + "RateLimitedFeedbackAngularAction",
"",
registry,
gains.getYoMaximumFeedbackRate(),
dt,
feedbackAngularAction);
parentRegistry.addChild(registry);
}
public KinematicsBasedFootSwitch(
String footName,
SideDependentList<? extends ContactablePlaneBody> bipedFeet,
double switchZThreshold,
double totalRobotWeight,
RobotSide side,
YoVariableRegistry parentRegistry) {
registry = new YoVariableRegistry(footName + getClass().getSimpleName());
foot = bipedFeet.get(side);
ContactablePlaneBody oppositeFoot = bipedFeet.get(side.getOppositeSide());
otherFeet = new ContactablePlaneBody[] {oppositeFoot};
this.totalRobotWeight = totalRobotWeight;
hitGround = new BooleanYoVariable(footName + "hitGround", registry);
fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
soleZ = new DoubleYoVariable(footName + "soleZ", registry);
ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
this.switchZThreshold.set(switchZThreshold);
yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);
parentRegistry.addChild(registry);
}
public TwoWaypointSwingGenerator(
String namePrefix,
double maxSwingHeight,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
parentRegistry.addChild(registry);
stepTime = new DoubleYoVariable(namePrefix + "StepTime", registry);
timeIntoStep = new DoubleYoVariable(namePrefix + "TimeIntoStep", registry);
isDone = new BooleanYoVariable(namePrefix + "IsDone", registry);
trajectoryType =
new EnumYoVariable<>(namePrefix + "TrajectoryType", registry, TrajectoryType.class);
swingHeight = new DoubleYoVariable(namePrefix + "SwingHeight", registry);
swingHeight.set(defaultSwingHeight);
this.maxSwingHeight = new DoubleYoVariable(namePrefix + "MaxSwingHeight", registry);
this.maxSwingHeight.set(maxSwingHeight);
trajectory =
new Position2dOptimizedSwingTrajectoryGenerator(
namePrefix, registry, yoGraphicsListRegistry, maxTimeIterations);
for (int i = 0; i < numberWaypoints; i++) waypointPositions.add(new FramePoint());
if (yoGraphicsListRegistry != null)
waypointViz =
new BagOfBalls(
numberWaypoints,
0.02,
namePrefix + "Waypoints",
YoAppearance.White(),
registry,
yoGraphicsListRegistry);
else waypointViz = null;
}
private ArrayList<YoVariable<?>> createALargeNumberOfVariables(
Random random, int numberOfVariables) {
YoVariableRegistry rootRegistry = new YoVariableRegistry("rootRegistry");
YoVariableRegistry registryOne = new YoVariableRegistry("registryOne");
YoVariableRegistry registryTwo = new YoVariableRegistry("registryTwo");
YoVariableRegistry registryThree = new YoVariableRegistry("registryThree");
rootRegistry.addChild(registryOne);
registryOne.addChild(registryTwo);
registryTwo.addChild(registryThree);
DoubleYoVariable t = new DoubleYoVariable("t", registryThree);
DoubleYoVariable time = new DoubleYoVariable("time", registryThree);
t.set(1.1);
time.set(2.2);
for (int i = 0; i < numberOfVariables; i++) {
DoubleYoVariable variable = new DoubleYoVariable("variable" + i, registryThree);
variable.set(Math.random());
}
return rootRegistry.getAllVariablesIncludingDescendants();
}
public CirclePoseTrajectoryGenerator(
String namePrefix,
ReferenceFrame trajectoryFrame,
DoubleProvider trajectoryTimeProvider,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
// registry);
this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
// registry);
this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;
this.trajectoryFrame = trajectoryFrame;
initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);
isCurrentAngleBeingAdjusted =
new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
maximumAngleTrackingErrorTolerated =
new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
currentControlledFrameRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
currentAngleTrackingError =
new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
currentAdjustedRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);
desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);
yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);
yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
yoCurrentVelocity =
new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
yoCurrentAcceleration =
new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);
yoInitialOrientation =
new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
yoFinalOrientation =
new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);
yoCurrentOrientation =
new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
yoCurrentAngularVelocity =
new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
yoCurrentAngularAcceleration =
new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);
yoInitialPositionWorld =
new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
yoFinalPositionWorld =
new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
yoCurrentPositionWorld =
new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
yoCurrentAdjustedPositionWorld =
new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);
circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
rotationAxis.set(0.0, 0.0, 1.0);
circleFrame =
new ReferenceFrame("CircleFrame", trajectoryFrame) {
private static final long serialVersionUID = 9102217353690768074L;
private final Vector3d localTranslation = new Vector3d();
private final Vector3d localRotationAxis = new Vector3d();
private final AxisAngle4d localAxisAngle = new AxisAngle4d();
@Override
protected void updateTransformToParent(RigidBodyTransform transformToParent) {
circleOrigin.get(localTranslation);
rotationAxis.get(localRotationAxis);
GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
transformToParent.set(localAxisAngle, localTranslation);
}
};
tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
yoTangentialCircleFramePose =
new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);
if (this.visualize && yoGraphicsListRegistry != null) {
final YoGraphicPosition currentPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
final YoGraphicPosition currentAdjustedPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentAdjustedPosition",
yoCurrentAdjustedPositionWorld,
0.023,
YoAppearance.Gold());
final YoGraphicPosition initialPositionViz =
new YoGraphicPosition(
namePrefix + "InitialPosition",
yoInitialPositionWorld,
0.02,
YoAppearance.BlueViolet());
final YoGraphicPosition finalPositionViz =
new YoGraphicPosition(
namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
final YoGraphicCoordinateSystem tangentialFrameViz =
new YoGraphicCoordinateSystem(
namePrefix + "TangentialFrame",
yoTangentialCircleFramePose,
0.2,
YoAppearance.Pink());
YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
yoGraphicsList.add(currentPositionViz);
yoGraphicsList.add(currentAdjustedPositionViz);
yoGraphicsList.add(initialPositionViz);
yoGraphicsList.add(finalPositionViz);
yoGraphicsList.add(tangentialFrameViz);
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
bagOfBalls =
new BagOfBalls(
numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);
showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
showViz.addVariableChangedListener(
new VariableChangedListener() {
public void variableChanged(YoVariable<?> v) {
boolean visible = showViz.getBooleanValue();
currentPositionViz.setVisible(visible);
currentAdjustedPositionViz.setVisible(visible);
initialPositionViz.setVisible(visible);
finalPositionViz.setVisible(visible);
tangentialFrameViz.setVisible(visible);
bagOfBalls.setVisible(visible);
}
});
showViz.notifyVariableChangedListeners();
} else {
visualize = false;
bagOfBalls = null;
showViz = null;
}
parentRegistry.addChild(registry);
}
@DeployableTestMethod
@Test(timeout = 300000)
public void testDataFileWriterAndReader() throws IOException, RepeatDataBufferEntryException {
int numDataPoints = 10000;
DataBuffer dataBuffer = new DataBuffer(numDataPoints);
YoVariableRegistry rootRegistry = new YoVariableRegistry("rootRegistry");
YoVariableRegistry registryOne = new YoVariableRegistry("registryOne");
YoVariableRegistry registryTwo = new YoVariableRegistry("registryTwo");
YoVariableRegistry registryThree = new YoVariableRegistry("registryThree");
rootRegistry.addChild(registryOne);
rootRegistry.addChild(registryTwo);
registryTwo.addChild(registryThree);
DoubleYoVariable variableOne = new DoubleYoVariable("variableOne", rootRegistry);
DoubleYoVariable variableTwo = new DoubleYoVariable("variableTwo", rootRegistry);
DoubleYoVariable variableThree = new DoubleYoVariable("variableThree", rootRegistry);
DoubleYoVariable variableFour = new DoubleYoVariable("variableFour", registryOne);
DoubleYoVariable variableFive = new DoubleYoVariable("variableFive", registryTwo);
BooleanYoVariable variableSix = new BooleanYoVariable("variableSix", rootRegistry);
IntegerYoVariable variableSeven = new IntegerYoVariable("variableSeven", registryThree);
dataBuffer.addVariable(variableOne);
dataBuffer.addVariable(variableTwo);
dataBuffer.addVariable(variableThree);
dataBuffer.addVariable(variableFour);
dataBuffer.addVariable(variableFive);
dataBuffer.addVariable(variableSix);
dataBuffer.addVariable(variableSeven);
for (int i = 0; i < numDataPoints; i++) {
variableOne.set(Math.random());
variableTwo.set(Math.random());
variableThree.set((int) (Math.random() * 100.0));
variableFour.set((int) (Math.random() * 100.0));
variableFive.set(Math.random());
variableSix.set(Math.random() > 0.5);
variableSeven.set((int) (Math.random() * 1000.0));
dataBuffer.tickAndUpdate();
}
Robot robot = new Robot("testRobot");
ArrayList<YoVariable<?>> allVariables = rootRegistry.getAllVariablesIncludingDescendants();
boolean binary = false;
boolean compress = false;
boolean spreadsheetFormatted = true;
testDataWriteReadIsTheSame(
dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
spreadsheetFormatted = false;
testDataWriteReadIsTheSame(
dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
binary = true;
compress = false;
testDataWriteReadIsTheSame(
dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
binary = false;
compress = true;
testDataWriteReadIsTheSame(
dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
binary = true;
compress = true;
testDataWriteReadIsTheSame(
dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
}
public FullRobotModelRootJointRewinder(
FullRobotModel fullRobotModel, YoVariableRegistry parentRegistry) {
this.fullRobotModel = fullRobotModel;
parentRegistry.addChild(registry);
}
public String getName() {
return registry.getName();
}
public static void setupJoyStickAndTreadmill(YoVariableRegistry registry) {
Joystick joystickUpdater;
try {
joystickUpdater = new Joystick();
} catch (IOException e) {
e.printStackTrace();
return;
}
double deadZone = 0.02;
final double desiredVelocityX_Bias = 0.0;
double desiredVelocityY_Bias = 0.0;
final double desiredHeadingDot_Bias = 0.0;
final double minHeadingDot = -0.1, minVelocityX = -0.2, maxHeadingDot = 0.1, maxVelocityX = 0.2;
boolean signFlip = true;
// Start the heading and speed updater thread
// --(!) Requires data from MultisenseHeadingSubscriber
final ValkyrieHeadingUpdater valkyrieHeadingUpdater =
new ValkyrieHeadingUpdater(
registry, minHeadingDot, maxHeadingDot, minVelocityX, maxVelocityX, signFlip);
final Thread headingUpdaterThread = new Thread(valkyrieHeadingUpdater);
headingUpdaterThread.start();
// --------------------
// Speed and Heading Controller
// --------------------
final BooleanYoVariable isMultisenseControllingSpeedAndHeading =
new BooleanYoVariable("isMultisenseControllingSpeedAndHeading", registry);
final DoubleYoVariable headingDotConstant =
new DoubleYoVariable("headingDotConstant", registry);
final DoubleYoVariable velocityXConstant = new DoubleYoVariable("velocityXConstant", registry);
final DoubleYoVariable desiredHeadingDot =
(DoubleYoVariable)
registry.getVariable("RateBasedDesiredHeadingControlModule", "desiredHeadingDot");
final DoubleYoVariable desiredVelocityX =
(DoubleYoVariable)
registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityX");
if (desiredVelocityX == null || joystickUpdater == null) return;
isMultisenseControllingSpeedAndHeading.set(false);
isMultisenseControllingSpeedAndHeading.addVariableChangedListener(
new VariableChangedListener() {
@Override
public void variableChanged(YoVariable<?> v) {
// Reset heading and speed when toggling controller
if (v.getValueAsDouble() != 0) {
desiredVelocityX.set(desiredVelocityX_Bias);
desiredHeadingDot.set(desiredHeadingDot_Bias);
}
}
});
headingDotConstant.set(0.5);
velocityXConstant.set(0.1);
desiredHeadingDot.set(desiredHeadingDot_Bias);
desiredHeadingDot.addVariableChangedListener(
new VariableChangedListener() {
@Override
public void variableChanged(YoVariable<?> v) {
// Overwrite joystick's input if it is disabled for heading control.
if (isMultisenseControllingSpeedAndHeading.getBooleanValue())
desiredHeadingDot.set(valkyrieHeadingUpdater.currentHeadingDot);
}
});
desiredVelocityX.set(desiredVelocityX_Bias);
joystickUpdater.addJoystickEventListener(
new DoubleYoVariableJoystickEventListener(
desiredVelocityX,
joystickUpdater.findComponent(Component.Identifier.Axis.Y),
minVelocityX + desiredVelocityX_Bias,
maxVelocityX + desiredVelocityX_Bias,
deadZone,
true));
desiredVelocityX.addVariableChangedListener(
new VariableChangedListener() {
@Override
public void variableChanged(YoVariable<?> v) {
// Overwrite joystick's input if it is disabled for speed control.
if (isMultisenseControllingSpeedAndHeading.getBooleanValue())
v.setValueFromDouble(valkyrieHeadingUpdater.currentVelocityX);
if (v.getValueAsDouble() < minVelocityX) v.setValueFromDouble(minVelocityX, false);
}
});
DoubleYoVariable desiredVelocityY =
(DoubleYoVariable)
registry.getVariable("ManualDesiredVelocityControlModule", "desiredVelocityY");
desiredVelocityY.set(desiredVelocityY_Bias);
joystickUpdater.addJoystickEventListener(
new DoubleYoVariableJoystickEventListener(
desiredVelocityY,
joystickUpdater.findComponent(Component.Identifier.Axis.X),
-0.1 + desiredVelocityY_Bias,
0.1 + desiredVelocityY_Bias,
deadZone,
true));
joystickUpdater.addJoystickEventListener(
new DoubleYoVariableJoystickEventListener(
desiredHeadingDot,
joystickUpdater.findComponent(Component.Identifier.Axis.RZ),
minHeadingDot + desiredHeadingDot_Bias,
maxHeadingDot + desiredHeadingDot_Bias,
deadZone,
signFlip));
BooleanYoVariable walk =
(BooleanYoVariable)
registry.getVariable("DesiredFootstepCalculatorFootstepProviderWrapper", "walk");
joystickUpdater.addJoystickEventListener(
new BooleanYoVariableJoystickEventListener(
walk, joystickUpdater.findComponent(Component.Identifier.Button.TRIGGER), true));
}
public void setupForLogging() {
logger = Logger.getLogger(registry.getName());
}
