@Override public void setPosition(Point3d position) { this.position.set(position); }
public YoSE3ConfigurationProvider(String name, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint(name, frame, registry); orientation = new YoFrameQuaternion(name, frame, registry); }
@Override public void getPosition(Point3d positionToPack) { position.get(positionToPack); }
/** * Put the constant CMP in the middle of the two given footsteps. * @param constantCMPToPack YoFramePoint that will be packed with the constant CMP location * @param firstFootstep FramePoint holding the position of the first footstep * @param secondFootstep FramePoint holding the position of the second footstep */ public static void putConstantCMPBetweenFeet(YoFramePoint constantCMPToPack, FramePoint firstFootstep, FramePoint secondFootstep) { constantCMPToPack.setAndMatchFrame(firstFootstep); constantCMPToPack.add(secondFootstep); constantCMPToPack.scale(0.5); }
public double distance(YoFramePoint yoFramePoint) { return distance(yoFramePoint.getFrameTuple()); }
private void setupSlidersForSupportBaseTargetControl() { int sliderChannel = 1; for (int i = 0; i < baseControlTargetPoints.length; i++) { YoFramePoint baseControlTargetPoint = baseControlTargetPoints[i]; sliderBoard.setSlider(sliderChannel++, baseControlTargetPoint.getYoX(), baseControlTargetPoint.getX() - 2, baseControlTargetPoint.getX() + 2); sliderBoard.setSlider(sliderChannel++, baseControlTargetPoint.getYoY(), baseControlTargetPoint.getY() - 2, baseControlTargetPoint.getY() + 2); } }
private void updateTrustedFootPosition(RigidBody trustedFoot) { YoFramePoint footPositionInWorld = footPositionsInWorld.get(trustedFoot); AlphaFilteredYoFrameVector footToRootJointPosition = footToRootJointPositions.get(trustedFoot); if (trustCoPAsNonSlippingContactPoint.getBooleanValue()) { footToRootJointPosition.getFrameTupleIncludingFrame(tempPosition); rootJointPosition.getFrameTupleIncludingFrame(tempFramePoint); tempFramePoint.sub(tempPosition); // New foot position footPositionInWorld.getFrameTuple(tempPosition); // Previous foot position tempFrameVector.sub(tempFramePoint, tempPosition); // Delta from previous to new foot position copPositionsInWorld.get(trustedFoot).add(tempFrameVector); // New CoP position } footPositionInWorld.set(rootJointPosition); footPositionInWorld.sub(footToRootJointPosition); }
@Override public void initialize() { currentTime.set(0.0); double tIntermediate = trajectoryTime.getDoubleValue() / 2.0; xPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getX(), finalPosition.getX(), finalVelocity.getX()); yPolynomial.setQuadraticWithFinalVelocityConstraint(0.0,trajectoryTime.getDoubleValue(), initialPosition.getY(), finalPosition.getY(), finalVelocity.getY()); zPolynomial.setCubicWithIntermediatePositionAndFinalVelocityConstraint(0.0, tIntermediate, trajectoryTime.getDoubleValue(), initialPosition.getZ(), intermediateZPosition.getDoubleValue(), finalPosition.getZ(), finalVelocity.getZ()); currentPosition.set(initialPosition); currentAcceleration.setToZero(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { yoFramePose.getOrientation().getQuaternion(rotation); transformToParent.setRotation(rotation); YoFramePoint yoFramePoint = yoFramePose.getPosition(); transformToParent.setTranslation(yoFramePoint.getX(), yoFramePoint.getY(), yoFramePoint.getZ()); }
FramePoint origin = new FramePoint(ReferenceFrame.getWorldFrame(), 0.5, -1.0, 2.0); vertices.set(RobotQuadrant.FRONT_LEFT, new YoFramePoint(RobotQuadrant.FRONT_LEFT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry)); vertices.set(RobotQuadrant.FRONT_RIGHT, new YoFramePoint(RobotQuadrant.FRONT_RIGHT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry)); vertices.set(RobotQuadrant.HIND_RIGHT, new YoFramePoint(RobotQuadrant.HIND_RIGHT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry)); vertices.set(RobotQuadrant.HIND_LEFT, new YoFramePoint(RobotQuadrant.HIND_LEFT.getCamelCaseNameForStartOfExpression(), ReferenceFrame.getWorldFrame(), registry)); vertices.get(RobotQuadrant.FRONT_LEFT).set(0.0, 1.0, 0.0); vertices.get(RobotQuadrant.FRONT_RIGHT).set(1.0, 1.0, 0.0); vertices.get(RobotQuadrant.HIND_RIGHT).set(1.0, 0.0, 0.0); vertices.get(RobotQuadrant.HIND_LEFT).set(0.0, 0.0, 0.0); vertices.get(RobotQuadrant.FRONT_LEFT).add(origin); vertices.get(RobotQuadrant.FRONT_RIGHT).add(origin); vertices.get(RobotQuadrant.HIND_RIGHT).add(origin); vertices.get(RobotQuadrant.HIND_LEFT).add(origin); vertices.get(RobotQuadrant.FRONT_LEFT).set(tempCommonSupportPolygon.getFootstep(RobotQuadrant.FRONT_LEFT)); vertices.get(RobotQuadrant.FRONT_RIGHT).set(tempCommonSupportPolygon.getFootstep(RobotQuadrant.FRONT_RIGHT)); vertices.remove(RobotQuadrant.HIND_RIGHT); vertices.get(RobotQuadrant.HIND_LEFT).set(tempCommonSupportPolygon.getFootstep(RobotQuadrant.HIND_LEFT));
private void updateCurrents() { if (numberOfFeetTrusted.getIntegerValue() == numberOfFeet) { for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); footPosition.setToZero(footSoleFrames.get(foot)); currentFootPositions.get(foot).setAndMatchFrame(footPosition); } currentAverageFootPosition.setToZero(); for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); currentAverageFootPosition.add(currentFootPositions.get(foot)); } currentAverageFootPosition.scale(1.0 / numberOfFeet); } else { for (int i = 0; i < numberOfFeet; i++) currentFootPositions.get(allFeet.get(i)).setToNaN(); currentAverageFootPosition.setToNaN(); } }
/** * Estimates the pelvis position and linear velocity using the leg kinematics * @param trustedFoot is the foot used to estimates the pelvis state * @param numberOfTrustedSides is only one or both legs used to estimate the pelvis state */ private void updatePelvisWithKinematics(RigidBody trustedFoot, int numberOfTrustedFeet) { double scaleFactor = 1.0 / numberOfTrustedFeet; footToRootJointPositions.get(trustedFoot).getFrameTuple(tempPosition); tempPosition.scale(scaleFactor); rootJointPosition.add(tempPosition); footPositionsInWorld.get(trustedFoot).getFrameTuple(tempPosition); tempPosition.scale(scaleFactor); rootJointPosition.add(tempPosition); YoFramePoint rootJointPositionPerFoot = rootJointPositionsPerFoot.get(trustedFoot); rootJointPositionPerFoot.set(footPositionsInWorld.get(trustedFoot)); rootJointPositionPerFoot.add(footToRootJointPositions.get(trustedFoot)); footVelocitiesInWorld.get(trustedFoot).getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.scale(scaleFactor * alphaRootJointLinearVelocityNewTwist.getDoubleValue()); rootJointLinearVelocityNewTwist.sub(tempFrameVector); }
private void computePositionFromMergingMeasurements() { yoRootJointPosition.getFrameTuple(rootJointPosition); imuBasedLinearStateCalculator.updatePelvisPosition(rootJointPosition, pelvisPositionIMUPart); kinematicsBasedLinearStateCalculator.getPelvisPosition(pelvisPositionKinPart); pelvisPositionIMUPart.scale(alphaIMUAgainstKinematicsForPosition.getDoubleValue()); pelvisPositionKinPart.scale(1.0 - alphaIMUAgainstKinematicsForPosition.getDoubleValue()); rootJointPosition.set(pelvisPositionIMUPart); rootJointPosition.add(pelvisPositionKinPart); yoRootJointPosition.set(rootJointPosition); }
public void update() { if (footRawCoPPositionsInWorld != null) { overallRawCoPPositionInWorld.setToZero(); double totalFootForce = 0.0; for (int i = 0; i < footList.size(); i++) { RigidBody rigidBody = footList.get(i); footSwitches.get(rigidBody).computeAndPackCoP(tempRawCoP2d); tempRawCoP.setIncludingFrame(tempRawCoP2d.getReferenceFrame(), tempRawCoP2d.getX(), tempRawCoP2d.getY(), 0.0); tempRawCoP.changeFrame(worldFrame); footRawCoPPositionsInWorld.get(rigidBody).set(tempRawCoP); footSwitches.get(rigidBody).computeAndPackFootWrench(tempWrench); double singleFootForce = tempWrench.getLinearPartZ(); totalFootForce += singleFootForce; tempRawCoP.scale(singleFootForce); overallRawCoPPositionInWorld.add(tempRawCoP); } overallRawCoPPositionInWorld.scale(1.0 / totalFootForce); } }
quadrupedSupportPolygon.setFootstep(quadrant, yoFootPositions.get(quadrant).getFrameTuple()); cartesianTrajectoryGenerator.getPosition(swingLegPosition); yoFootPositions.get(robotQuadrant).set(swingLegPosition); swingLeg.set(robotQuadrant); FramePoint initialPosition = new FramePoint(yoFootPositions.get(robotQuadrant).getFramePointCopy()); FramePoint desiredFootPosition = new FramePoint(); cartesianTrajectoryGenerator.setTrajectoryParameters(desiredSwingTime.getDoubleValue(), initialPosition, swingHeight.getDoubleValue(), desiredFootPosition, finalDesiredVelocity.getFrameTuple()); cartesianTrajectoryGenerator.initialize(); swingTarget.set(desiredFootPosition); FramePoint footPosition = yoFootPosition.getFramePointCopy(); footPosition.changeFrame(ReferenceFrame.getWorldFrame()); fourFootSupportPolygon.setFootstep(robotQuadrant, footPosition); centroid.set(temporaryCentroid); nominalYaw.set(fourFootSupportPolygon.getNominalYaw()); endPoint.add(0.4,0.0,0.0); endPoint.yawAboutPoint(temporaryCentroid, endPoint, nominalYaw.getDoubleValue()); nominalYawEndpoint.set(endPoint); nominalYawLineSegment.set(centroid.getFramePoint2dCopy(), endpointTwoD); nominalYawGraphic.update();
private void setAccelerationEndpointPositions() { for (int i : new int[] {0, 1}) { FrameVector waypointToEndpoint = getWaypointToEndpoint(i); FrameVector oppositeWaypointToEndpoint = getOppositeWaypointToEndpoint(i); double scaleFactor = waypointToEndpoint.dot(oppositeWaypointToEndpoint) / oppositeWaypointToEndpoint.length() * linearSplineLengthFactor.getDoubleValue(); oppositeWaypointToEndpoint.normalize(); oppositeWaypointToEndpoint.scale(scaleFactor); allPositions[accelerationEndpointIndices[i]].set(allPositions[waypointIndices[i]].getFramePointCopy()); allPositions[accelerationEndpointIndices[i]].add(oppositeWaypointToEndpoint); } }
/** * GC-free but unsafe accessor. */ public FramePoint getPosition() { return position.getFrameTuple(); }
protected void checkForCloseWaypoints() { if (waypointsAreCloseTogether()) { FramePoint midpoint = allPositions[waypointIndices[0]].getFramePointCopy(); midpoint.add(allPositions[waypointIndices[1]].getFramePointCopy()); midpoint.scale(0.5); for (int i = 0; i < 2; i++) { allPositions[waypointIndices[i]].set(midpoint); } waypointsAreTheSamePoint = true; } else { waypointsAreTheSamePoint = false; } }
public void initialize() { time.set(0.0); FramePoint positionToPack = new FramePoint(); positionProvider.getPosition(positionToPack); positionToPack.changeFrame(position.getReferenceFrame()); position.set(positionToPack); }