/** * Returns the average of two 3D points. * * @param a the first 3D point. Not modified. * @param b the second 3D point. Not modified. * @param avgToPack the point in which the computed average is stored. Modified. */ public static void averagePoints(FramePoint a, FramePoint b, FramePoint avgToPack) { avgToPack.setIncludingFrame(a); avgToPack.add(b); avgToPack.scale(0.5); }
public static FramePoint average(List<? extends FramePoint> framePoints) { ReferenceFrame frame = framePoints.get(0).getReferenceFrame(); FramePoint nextPoint = new FramePoint(frame); FramePoint average = new FramePoint(framePoints.get(0)); for (int i = 1; i < framePoints.size(); i++) { nextPoint.set(framePoints.get(i)); nextPoint.changeFrame(frame); average.add(nextPoint); } average.scale(1.0 / ((double) framePoints.size())); return average; }
private void updateCenterOfMassPosition(FrameVector centerOfMassPositionDelta) { centerOfMassPosition.set(centerOfMassPositionPort.getData()); centerOfMassPosition.add(centerOfMassPositionDelta); centerOfMassPositionPort.setData(centerOfMassPosition); } }
private void updateCenterOfMassVelocity(FrameVector centerOfMassPositionDelta) { centerOfMassPosition.set(centerOfMassPositionPort.getData()); centerOfMassPosition.add(centerOfMassPositionDelta); centerOfMassPositionPort.setData(centerOfMassPosition); } }
public void getPosition(FramePoint positionToPack, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); positionToPack.setToZero(referenceFrame); // c2 * q^2 c2.getFrameTuple(positionToPack); positionToPack.scale(MathTools.square(q)); // c1 * q c1.getFrameTuple(tempPackPosition); tempPackPosition.scale(q); positionToPack.add(tempPackPosition); // c0 c0.getFrameTuple(tempPackPosition2); positionToPack.add(tempPackPosition2); }
public void pitchAboutPoint(FramePoint pointToPitchAbout, FramePoint resultToPack, double pitch) { checkReferenceFrameMatch(pointToPitchAbout); double tempX = getX() - pointToPitchAbout.getX(); double tempY = getY() - pointToPitchAbout.getY(); double tempZ = getZ() - pointToPitchAbout.getZ(); double cosAngle = Math.cos(pitch); double sinAngle = Math.sin(pitch); double x = cosAngle * tempX + sinAngle * tempZ; tempZ = -sinAngle * tempX + cosAngle * tempZ; tempX = x; resultToPack.setIncludingFrame(pointToPitchAbout); resultToPack.add(tempX, tempY, tempZ); }
public static List<FramePoint> getWaypointsAtSpecifiedGroundClearance(FramePoint initialPosition, FramePoint finalPosition, double groundClearance, double[] proportionsThroughTrajectoryForGroundClearance) { List<FramePoint> waypoints = new ArrayList<FramePoint>(); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); for (int i = 0; i < 2; i++) { FramePoint waypoint = waypoints.get(i); waypoint.set(initialPosition); FrameVector offsetFromInitial = new FrameVector(waypoint.getReferenceFrame()); offsetFromInitial.set(finalPosition); offsetFromInitial.sub(initialPosition); offsetFromInitial.scale(proportionsThroughTrajectoryForGroundClearance[i]); waypoint.add(offsetFromInitial); waypoint.setZ(waypoints.get(i).getZ() + groundClearance); } return waypoints; }
returnPoint.add(vectorBetweenPoints);
for (int i = 0; i < childrenJoints.size(); i++) curChildCoMScaledByMass.add(getCoMScaledByMass(childrenJoints.get(i).getSuccessor()));
/** * yawAboutPoint * * @param pointToYawAbout FramePoint * @param yaw double * @return CartesianPositionFootstep */ public void yawAboutPoint(FramePoint pointToYawAbout, FramePoint resultToPack, double yaw) { checkReferenceFrameMatch(pointToYawAbout); double tempX = getX() - pointToYawAbout.getX(); double tempY = getY() - pointToYawAbout.getY(); double tempZ = getZ() - pointToYawAbout.getZ(); double cosAngle = Math.cos(yaw); double sinAngle = Math.sin(yaw); double x = cosAngle * tempX + -sinAngle * tempY; tempY = sinAngle * tempX + cosAngle * tempY; tempX = x; resultToPack.setIncludingFrame(pointToYawAbout); resultToPack.add(tempX, tempY, tempZ); }
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; } }
waypoint.add(planarWaypointOffset);
public void compute() { centerOfMass.setToZero(desiredFrame); totalMass = 0.0; for (RigidBody rigidBody : rigidBodies) { rigidBody.getCoMOffset(tempPoint); double mass = rigidBody.getInertia().getMass(); tempPoint.changeFrame(desiredFrame); tempPoint.scale(mass); centerOfMass.add(tempPoint); totalMass += mass; } centerOfMass.scale(1.0 / totalMass); }
waypointPositions.get(i).add(0.0, 0.0, swingHeight.getDoubleValue());
private List<FramePoint> getWaypointsAtGroundClearances(double[] groundClearances, double[] proportionsThroughTrajectoryForGroundClearance) { FramePoint initialPosition = allPositions[0].getFramePointCopy(); FramePoint finalPosition = allPositions[3].getFramePointCopy(); positionSources[0].getPosition(initialPosition); positionSources[1].getPosition(finalPosition); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); List<FramePoint> waypoints = new ArrayList<FramePoint>(); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); for (int i = 0; i < 2; i++) { FramePoint waypoint = waypoints.get(i); waypoint.set(initialPosition); FrameVector offsetFromInitial = new FrameVector(waypoint.getReferenceFrame()); offsetFromInitial.set(finalPosition); offsetFromInitial.sub(initialPosition); offsetFromInitial.scale(proportionsThroughTrajectoryForGroundClearance[i]); waypoint.add(offsetFromInitial); waypoint.setZ(waypoints.get(i).getZ() + groundClearances[i]); } return waypoints; }
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() { FramePoint tempCoMPosition = new FramePoint(worldFrame); for (RigidBodyVisualizationData comData : centerOfMassData) { comData.rigidBody.getCoMOffset(tempCoMPosition); tempCoMPosition.changeFrame(worldFrame); tempCoMPosition.add(inertiaEllipsoidGhostOffset.getFrameVectorCopy()); FrameOrientation orientation = new FrameOrientation(comData.rigidBody.getBodyFixedFrame()); orientation.changeFrame(worldFrame); comData.position.set(tempCoMPosition); comData.orientation.set(orientation); } }
smoothedPosition.add(tempVector);
pointOnY.setIncludingFrame(trajectoryOrigin); direction.sub(this.finalPosition, this.initialPosition); pointOnY.add(direction.getY(), -direction.getX(), 0.0); swingFrame.setPoints(trajectoryOrigin, pointOnX, pointOnY); swingFrame.update();
endPoint.add(0.4,0.0,0.0); endPoint.yawAboutPoint(temporaryCentroid, endPoint, nominalYaw.getDoubleValue()); nominalYawEndpoint.set(endPoint);