public double getSwingHeight(FramePose startPose, FramePose endPose, double stanceHeight, HeightMapWithPoints groundProfile) { FramePoint startFramePoint = startPose.getFramePointCopy(); FramePoint endFramePoint = endPose.getFramePointCopy(); Point3d startPoint = startFramePoint.getPointCopy(); Point3d endPoint = endFramePoint.getPointCopy(); return getSwingHeight(startPoint, endPoint, stanceHeight, groundProfile); }
@Override public void onNewMessage(PositionMeasurementArray message) { Object[] people = message.getPeople().toArray(); String[] ids = new String[people.length]; Point3d[] positions = new Point3d[people.length]; for(int i = 0; i < people.length; i++) { PositionMeasurement person = (PositionMeasurement) people[i]; ids[i] = String.valueOf(i); framePoint.setIncludingFrame(cameraFrame, person.getPos().getX(), person.getPos().getY(), person.getPos().getZ()); framePoint.changeFrame(ReferenceFrame.getWorldFrame()); positions[i] = framePoint.getPointCopy(); } DetectedFacesPacket detectedFaces = new DetectedFacesPacket(ids, positions); packetCommunicator.send(detectedFaces); } }
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetHandPoseToActual.getBooleanValue()) { ReferenceFrame referenceFrame = getReferenceFrameToUse(); ReferenceFrame wristFrame = fullRobotModel.getEndEffectorFrame(userHandPoseSide.getEnumValue(), LimbName.ARM); FramePose currentPose = new FramePose(wristFrame); currentPose.changeFrame(referenceFrame); userDesiredHandPose.setPosition(currentPose.getFramePointCopy().getPointCopy()); userDesiredHandPose.setOrientation(currentPose.getFrameOrientationCopy().getQuaternionCopy()); userDesiredSetHandPoseToActual.set(false); } } });
private void computeCoMPositionError() { Point3d comPoint = new Point3d(); Vector3d linearVelocity = new Vector3d(); Vector3d angularMomentum = new Vector3d(); robot.computeCOMMomentum(comPoint, linearVelocity, angularMomentum); perfectCoMPosition.set(comPoint); Vector3d comError = new Vector3d(); orientationEstimator.getEstimatedCoMPosition(estimatedCoMPosition); comError.set(estimatedCoMPosition.getPointCopy()); comError.sub(comPoint); comZPositionError.set(comError.getZ()); comError.setZ(0.0); comXYPositionError.set(comError.length()); }
Point3d startPoint = startFramePoint.getPointCopy(); Point3d endPoint = endFramePoint.getPointCopy();