public void setY(double y) { position.setY(y); }
public void setY(double y) { position.setY(y); }
private void updateFocus(FramePoint point) { double x = point.getX(); double y = point.getY(); if (firstFocus) { focusXmin = x; focusYmin = y; focusXmax = x; focusYmax = y; firstFocus = false; } else { focusXmin = Math.min(focusXmin, x); focusYmin = Math.min(focusYmin, y); focusXmax = Math.max(focusXmax, x); focusYmax = Math.max(focusYmax, y); } focusX = (focusXmax + focusXmin) / 2.0; focusY = (focusYmax + focusYmin) / 2.0; if (textPoint != null) { textPoint.setX(focusX); textPoint.setY(focusYmax + 0.2); } }
public void setDesiredCapturePointState(FramePoint2d currentDesiredCapturePointPosition, FrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
public void setDesiredCapturePointState(YoFramePoint2d currentDesiredCapturePointPosition, YoFrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
public void computePositionsForVis(double time) { nominalTrajectoryGenerator.compute(time); xPolynomial.compute(time); yPolynomial.compute(time); nominalTrajectoryGenerator.getPosition(nominalTrajectoryPosition); nominalTrajectoryGenerator.getVelocity(nominalTrajectoryVelocity); nominalTrajectoryGenerator.getAcceleration(nominalTrajectoryAcceleration); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); }
public void computeWrenchFromRho(int startIndex, DenseMatrix64F allRobotRho) { CommonOps.extract(allRobotRho, startIndex, startIndex + rhoSize, 0, 1, rhoMatrix, 0, 0); yoRho.set(rhoMatrix); if (yoPlaneContactState.inContact()) { totalWrenchMatrix.zero(); for (int rhoIndex = 0; rhoIndex < rhoSize; rhoIndex++) { double rho = rhoMatrix.get(rhoIndex, 0); CommonOps.extract(rhoJacobianMatrix, 0, SpatialForceVector.SIZE, rhoIndex, rhoIndex + 1, singleRhoWrenchMatrix, 0, 0); MatrixTools.addMatrixBlock(totalWrenchMatrix, 0, 0, singleRhoWrenchMatrix, 0, 0, SpatialForceVector.SIZE, 1, rho); } RigidBody rigidBody = yoPlaneContactState.getRigidBody(); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchFromRho.set(bodyFixedFrame, centerOfMassFrame, totalWrenchMatrix); CommonOps.mult(copJacobianMatrix, rhoMatrix, previousCoPMatrix); previousCoP.setX(previousCoPMatrix.get(0, 0)); previousCoP.setY(previousCoPMatrix.get(1, 0)); } else { wrenchFromRho.setToZero(); } }
public void compute(double time) { timeIntoStep.set(time); nominalTrajectoryGenerator.compute(time); nominalTrajectoryGenerator.getLinearData(nominalTrajectoryPosition, nominalTrajectoryVelocity, nominalTrajectoryAcceleration); xPolynomial.compute(time); yPolynomial.compute(time); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); desiredVelocity.setX(xPolynomial.getVelocity()); desiredVelocity.setY(yPolynomial.getVelocity()); desiredVelocity.setZ(nominalTrajectoryVelocity.getZ()); desiredAcceleration.setX(xPolynomial.getAcceleration()); desiredAcceleration.setY(yPolynomial.getAcceleration()); desiredAcceleration.setZ(nominalTrajectoryAcceleration.getZ()); }