public void addPointInContact(FramePoint2d newPointInContact) { contactPoints.add().setXYIncludingFrame(newPointInContact); }
public void setPoint2dsInContact(ReferenceFrame contactFrame, List<Point2d> newPointsInContact) { clearContactPoints(); for (int i = 0; i < newPointsInContact.size(); i++) contactPoints.add().setXYIncludingFrame(contactFrame, newPointsInContact.get(i)); }
public void getFrameVertexXY(int vertexIndex, FramePoint vertexToPack) { convexPolygon.checkIfUpToDate(); convexPolygon.checkNonEmpty(); convexPolygon.checkIndexInBoundaries(vertexIndex); vertexToPack.setXYIncludingFrame(referenceFrame, convexPolygon.getVertex(vertexIndex)); }
private void addPredictedContactPointsToPolygon(Footstep footstep, FrameConvexPolygon2d convexPolygonToExtend) { List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); if (predictedContactPoints != null && !predictedContactPoints.isEmpty()) { int numberOfContactPoints = predictedContactPoints.size(); for (int i = 0; i < numberOfContactPoints; i++) { tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), predictedContactPoints.get(i)); convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint); } } else { ConvexPolygon2d defaultPolygon = defaultFootPolygons.get(footstep.getRobotSide()); for (int i = 0; i < defaultPolygon.getNumberOfVertices(); i++) { tempFramePoint.setXYIncludingFrame(footstep.getSoleReferenceFrame(), defaultPolygon.getVertex(i)); convexPolygonToExtend.addVertexByProjectionOntoXYPlane(tempFramePoint); } } }
private void findProjectionOntoPlaneFrame(ReferenceFrame planeFrame, FramePoint2d pointInWorld, FramePoint pointProjectedOntoPlaneFrameToPack) { pointInWorld.checkReferenceFrameMatch(worldFrame); double z = getPlaneZGivenXY(planeFrame, pointInWorld.getX(), pointInWorld.getY()); pointProjectedOntoPlaneFrameToPack.setXYIncludingFrame(pointInWorld); pointProjectedOntoPlaneFrameToPack.setZ(z); }
public void compute(Map<RigidBody, Wrench> externalWrenches) { for (int i = 0; i < contactablePlaneBodies.size(); i++) { ContactablePlaneBody contactablePlaneBody = contactablePlaneBodies.get(i); FramePoint2d cop = cops.get(contactablePlaneBody); YoFramePoint2d yoCop = yoCops.get(contactablePlaneBody); yoCop.getFrameTuple2d(cop); Wrench wrench = externalWrenches.get(contactablePlaneBody.getRigidBody()); if (wrench != null) { wrench.getLinearPartIncludingFrame(tempForce); double normalTorque = centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(cop, wrench, contactablePlaneBody.getSoleFrame()); centersOfPressure2d.get(contactablePlaneBody).set(cop); tempCoP3d.setXYIncludingFrame(cop); centersOfPressureWorld.get(contactablePlaneBody).setAndMatchFrame(tempCoP3d); groundReactionForceMagnitudes.get(contactablePlaneBody).set(tempForce.length()); normalTorques.get(contactablePlaneBody).set(normalTorque); } else { groundReactionForceMagnitudes.get(contactablePlaneBody).set(0.0); centersOfPressureWorld.get(contactablePlaneBody).setToNaN(); cop.setToNaN(); } yoCop.set(cop); desiredCenterOfPressureDataHolder.setCenterOfPressure(cop, contactablePlaneBody.getRigidBody()); } }
private void computeFinalCMPBetweenSupportFeet(int cmpIndex, FrameConvexPolygon2d footA, FrameConvexPolygon2d footB) { footA.getCentroid(tempCentroid); firstCMP.setXYIncludingFrame(tempCentroid); firstCMP.changeFrame(worldFrame); footB.getCentroid(tempCentroid); secondCMP.setXYIncludingFrame(tempCentroid); secondCMP.changeFrame(worldFrame); upcomingSupport.clear(worldFrame); tempFootPolygon.setIncludingFrame(footA); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); tempFootPolygon.setIncludingFrame(footB); tempFootPolygon.changeFrameAndProjectToXYPlane(worldFrame); upcomingSupport.addVertices(tempFootPolygon); upcomingSupport.update(); entryCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); exitCMPs.get(cmpIndex).switchCurrentReferenceFrame(worldFrame); upcomingSupport.getCentroid(tempCentroid); tempCentroid3d.setXYIncludingFrame(tempCentroid); double chicken = MathTools.clipToMinMax(percentageChickenSupport.getDoubleValue(), 0.0, 1.0); if (chicken <= 0.5) entryCMPs.get(cmpIndex).interpolate(firstCMP, tempCentroid3d, chicken * 2.0); else entryCMPs.get(cmpIndex).interpolate(tempCentroid3d, secondCMP, (chicken-0.5) * 2.0); exitCMPs.get(cmpIndex).set(entryCMPs.get(cmpIndex)); }
private void updateKinematicsNewTwist() { rootJoint.getJointTwist(tempRootBodyTwist); rootJointLinearVelocityNewTwist.getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.changeFrame(tempRootBodyTwist.getExpressedInFrame()); tempRootBodyTwist.setLinearPart(tempFrameVector); rootJoint.setJointTwist(tempRootBodyTwist); twistCalculator.compute(); for (int i = 0; i < feetRigidBodies.size(); i++) { RigidBody foot = feetRigidBodies.get(i); Twist footTwistInWorld = footTwistsInWorld.get(foot); YoFrameVector footVelocityInWorld = footVelocitiesInWorld.get(foot); twistCalculator.getTwistOfBody(footTwistInWorld, foot); footTwistInWorld.changeBodyFrameNoRelativeTwist(soleFrames.get(foot)); footTwistInWorld.changeFrame(soleFrames.get(foot)); this.copsFilteredInFootFrame.get(foot).getFrameTuple2dIncludingFrame(tempCoP2d); tempCoP.setXYIncludingFrame(tempCoP2d); footTwistInWorld.changeFrame(footTwistInWorld.getBaseFrame()); tempCoP.changeFrame(footTwistInWorld.getExpressedInFrame()); footTwistInWorld.getLinearVelocityOfPointFixedInBodyFrame(tempFrameVector, tempCoP); tempFrameVector.changeFrame(worldFrame); footVelocityInWorld.set(tempFrameVector); } }
computeEntryCMPForSupportFoot(cmp, supportSide, null, null); else cmp.setXYIncludingFrame(firstEntryCMPForSingleSupport);
private void intersectASinglePolygon(FrameConvexPolygon2d polygon, Pair<FramePoint, FramePoint> intersectionWithPolygon) { intersectionOfPlanes.changeFrame(polygon.getReferenceFrame()); intersectionOfPlanes.projectOntoXYPlane(planeIntersectionOnPolygonPlane); polygon.intersectionWith(planeIntersectionOnPolygonPlane, lineIntersectionOnPolygonPlane); if (lineIntersectionOnPolygonPlane.getFirst().containsNaN() && lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { noIntersection = true; } if (!lineIntersectionOnPolygonPlane.getFirst().containsNaN()) { intersectionWithPolygon.getFirst().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getFirst()); } if (!lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { intersectionWithPolygon.getSecond().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getSecond()); } }
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); footPolygon.clear(soleFrame); for (int i = 0; i < contactPoints.size(); i++) { contactPoints.get(i).getPosition2d(toeOffContactPoint2d); footPolygon.addVertex(toeOffContactPoint2d); } footPolygon.update(); FramePoint2d rayOrigin; if (!exitCMP2d.containsNaN() && footPolygon.isPointInside(exitCMP2d)) rayOrigin = exitCMP2d; else rayOrigin = footPolygon.getCentroid(); rayThroughExitCMP.set(rayOrigin, exitCMPRayDirection2d); frameConvexPolygonWithRayIntersector2d.intersectWithRay(footPolygon, rayThroughExitCMP); toeOffContactPoint2d.set(frameConvexPolygonWithRayIntersector2d.getIntersectionPointOne()); contactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); contactPointPosition.changeFrame(contactableFoot.getRigidBody().getBodyFixedFrame()); pointFeedbackControlCommand.setBodyFixedPointToControl(contactPointPosition); desiredContactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); desiredContactPointPosition.changeFrame(worldFrame); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredYawToHold = desiredOrientation.getYaw(); desiredRollToHold = desiredOrientation.getRoll(); }
private ReferenceFrame createToeFrame(RobotSide robotSide) { ContactableFoot contactableFoot = momentumBasedController.getContactableFeet().get(robotSide); ReferenceFrame footFrame = momentumBasedController.getReferenceFrames().getFootFrame(robotSide); FramePoint2d toeContactPoint2d = new FramePoint2d(); contactableFoot.getToeOffContactPoint(toeContactPoint2d); FramePoint toeContactPoint = new FramePoint(); toeContactPoint.setXYIncludingFrame(toeContactPoint2d); toeContactPoint.changeFrame(footFrame); transformFromToeToAnkle.setTranslation(toeContactPoint.getVectorCopy()); return ReferenceFrame.constructFrameWithUnchangingTransformToParent(robotSide.getCamelCaseNameForStartOfExpression() + "ToeFrame", footFrame, transformFromToeToAnkle); }
private boolean applyLocationDeadband(FramePoint2d solutionLocationToPack, FramePoint2d currentSolutionLocation, FramePoint2d referenceLocation2d, ReferenceFrame deadbandFrame, double deadband, double deadbandResolution) solutionLocation.setXYIncludingFrame(solutionLocationToPack); referenceLocation.setXYIncludingFrame(referenceLocation2d); previousLocation.setXYIncludingFrame(currentSolutionLocation);
if (!cop2d.containsNaN()) tempCoP3d.setXYIncludingFrame(cop2d); tempCoP3d.changeFrame(centerOfMassFrame); deltaZ = -tempCoP3d.getZ(); pseudoCoP2d.setToZero(copToCoPFrame); centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(pseudoCoP2d, totalGroundReactionWrench, copToCoPFrame); pseudoCoP.setXYIncludingFrame(pseudoCoP2d); pseudoCoP.changeFrame(centerOfMassFrame); deltaZ = -pseudoCoP.getZ();
private void computeEntryCMP(FramePoint entryCMPToPack, RobotSide robotSide, ReferenceFrame soleFrame, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { if (useTwoCMPsPerSupport) { if (centroidInSoleFrameOfPreviousSupportFoot != null) centroidOfFootstepToConsider.setIncludingFrame(centroidInSoleFrameOfPreviousSupportFoot); else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame); if (previousExitCMP != null) { previousExitCMP.getFrameTuple2dIncludingFrame(previousExitCMP2d); previousExitCMP2d.changeFrameAndProjectToXYPlane(soleFrame); // Choose the laziest option if (Math.abs(previousExitCMP2d.getX()) < Math.abs(centroidOfFootstepToConsider.getX())) centroidOfFootstepToConsider.set(previousExitCMP2d); } constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, entryCMPUserOffsets.get(robotSide), minForwardEntryCMPOffset.getDoubleValue(), maxForwardEntryCMPOffset.getDoubleValue()); } else { cmp2d.setIncludingFrame(footSupportPolygon.getCentroid()); YoFrameVector2d offset = entryCMPUserOffsets.get(robotSide); cmp2d.add(offset.getX(), offset.getY()); } entryCMPToPack.setXYIncludingFrame(cmp2d); entryCMPToPack.changeFrame(worldFrame); }
cop.setXYIncludingFrame(centerOfPressure); cop.changeFrame(worldFrame);
exitCMPToPack.setXYIncludingFrame(cmp2d); exitCMPToPack.changeFrame(worldFrame);
desiredCoP3d.setXYIncludingFrame(cop); // used to be desiredCoP desiredCoP3d.changeFrame(bodyFixedControlledPose.getReferenceFrame()); bodyFixedControlledPose.setPosition(desiredCoP3d);
if (cop2d.containsNaN()) cop2d.setToZero(contactableFoot.getSoleFrame()); framePosition.setXYIncludingFrame(cop2d); frameOrientation.setToZero(contactableFoot.getSoleFrame()); controlFrame.setPoseAndUpdate(framePosition, frameOrientation); desiredCopPosition.setXYIncludingFrame(cop2d); desiredCopPosition.setIncludingFrame(desiredSoleFrame, desiredCopPosition.getPoint()); desiredCopPosition.changeFrame(worldFrame);
projectedCapturePoint.setXYIncludingFrame(this.capturePoint2d);