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); } }
public double getY() { return getPosition().getY(); }
private void setupSlidersForSupportBaseControl() { int sliderChannel = 1; for (int i = 0; i < baseControlPoints.length; i++) { YoFramePoint baseControlPoint = baseControlPoints[i]; sliderBoard.setSlider(sliderChannel++, baseControlPoint.getYoX(), baseControlPoint.getX() - 2, baseControlPoint.getX() + 2); sliderBoard.setSlider(sliderChannel++, baseControlPoint.getYoY(), baseControlPoint.getY() - 2, baseControlPoint.getY() + 2); } }
public double getY() { return getPosition().getY(); }
private void setupSlidersForSupportBaseControl() { int sliderChannel = 1; for (int i = 0; i < baseControlPoints.length; i++) { YoFramePoint baseControlPoint = baseControlPoints[i]; sliderBoard.setSlider(sliderChannel++, baseControlPoint.getYoX(), baseControlPoint.getX() - 2, baseControlPoint.getX() + 2); sliderBoard.setSlider(sliderChannel++, baseControlPoint.getYoY(), baseControlPoint.getY() - 2, baseControlPoint.getY() + 2); } }
public void getProjectedOntoXYPlane(YoFramePoint2d positionToPack) { positionToPack.set(currentPosition.getX(), currentPosition.getY()); }
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); } }
@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()); }
@Override protected void computeRotationTranslation(Transform3d transform3D) { transform3D.setIdentity(); translationVector.set(yoFramePoint.getX(), yoFramePoint.getY(), yoFramePoint.getZ()); yoFrameOrientation.getEulerAngles(rotationEulerVector); transform3D.setRotationEulerAndZeroTranslation(rotationEulerVector); transform3D.setTranslation(translationVector); transform3D.setScale(scale); }
private void initializePolynomials() { xPolynomial.setInitialPositionVelocityZeroFinalHighOrderDerivatives(0.0, trajectoryTime.getDoubleValue(), initialPosition.getX(), initialVelocity.getX(), finalPosition.getX(), finalVelocity.getX()); yPolynomial.setInitialPositionVelocityZeroFinalHighOrderDerivatives(0.0, trajectoryTime.getDoubleValue(), initialPosition.getY(), initialVelocity.getY(), finalPosition.getY(), finalVelocity.getY()); zPolynomial.setInitialPositionVelocityZeroFinalHighOrderDerivatives(0.0, trajectoryTime.getDoubleValue(), initialPosition.getZ(), initialVelocity.getZ(), finalPosition.getZ(), finalVelocity.getZ()); }
@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(); }
public void set(YoFramePoint base, YoFrameVector vector) { this.baseX.set(base.getX()); this.baseY.set(base.getY()); this.baseZ.set(base.getZ()); this.x.set(vector.getX()); this.y.set(vector.getY()); this.z.set(vector.getZ()); }
endPose.setPosition(new FramePoint2d(worldFrame, targetLocation.getX(), targetLocation.getY())); endPose.setOrientation(new FrameOrientation2d(worldFrame, targetOrientation.getYaw().getDoubleValue()));
desiredCoPMatrix.set(1, 0, desiredCoP.getY()); desiredCoPWeightMatrix.set(0, 0, desiredCoPWeight.getX()); desiredCoPWeightMatrix.set(1, 1, desiredCoPWeight.getY());