@Override public void getPosition(FramePoint positionToPack) { positionToPack.setIncludingFrame(framePoint); } }
@Override public void getPosition(FramePoint positionToPack) { positionToPack.setIncludingFrame(desiredICPOutput); }
public static void getPosition(FramePoint pointToPack, ReferenceFrame frame, double angle, double radius, double z) { double x = radius * Math.cos(angle); double y = radius * Math.sin(angle); pointToPack.setIncludingFrame(frame, x, y, z); }
public void setPoints(FramePoint p1, FramePoint p2, FramePoint p3) { this.p1.setIncludingFrame(p1); this.p2.setIncludingFrame(p2); this.p3.setIncludingFrame(p3); this.p1.changeFrame(parentFrame); this.p2.changeFrame(parentFrame); this.p3.changeFrame(parentFrame); }
public void setFinalConditions(FramePoint finalPosition, FrameVector finalVelocity) { this.finalPosition.setIncludingFrame(finalPosition); this.finalVelocity.setIncludingFrame(finalVelocity); }
public void setInitialConditions(FramePoint initialPosition, FrameVector initialVelocity) { this.initialPosition.setIncludingFrame(initialPosition); this.initialVelocity.setIncludingFrame(initialVelocity); }
public void getSecondEndpointIncludingFrame(FramePoint secondEndpointToPack) { secondEndpointToPack.setIncludingFrame(referenceFrame, getSecondEndpoint()); }
public void addPointInContact(FramePoint newPointInContact) { contactPoints.add().setIncludingFrame(newPointInContact); }
public void getCenterOfMassOffset(FramePoint centerOfMassOffsetToPack) { centerOfMassOffsetToPack.setIncludingFrame(expressedInframe, crossPart); centerOfMassOffsetToPack.scale(-1.0 / mass); // comOffset = -1/m * c }
public void getIncludingFrame(FramePoint desiredPositionToPack, FrameVector desiredLinearVelocityToPack, FrameVector feedForwardLinearAccelerationToPack) { desiredPositionToPack.setIncludingFrame(worldFrame, desiredPositionInWorld); desiredLinearVelocityToPack.setIncludingFrame(worldFrame, desiredLinearVelocityInWorld); feedForwardLinearAccelerationToPack.setIncludingFrame(worldFrame, feedForwardLinearAccelerationInWorld); }
public void setExitCMP(FramePoint exitCMP) { this.exitCMP.setIncludingFrame(exitCMP); this.exitCMP.changeFrame(soleFrame); exitCMP2d.setByProjectionOntoXYPlaneIncludingFrame(this.exitCMP); }
public void getIncludingFrame(FramePoint desiredPositionToPack, FrameVector desiredLinearVelocityToPack, FrameVector feedForwardLinearAccelerationToPack) { desiredPositionToPack.setIncludingFrame(worldFrame, desiredPositionInWorld); desiredLinearVelocityToPack.setIncludingFrame(worldFrame, desiredLinearVelocityInWorld); feedForwardLinearAccelerationToPack.setIncludingFrame(worldFrame, feedForwardLinearAccelerationInWorld); }
public void getFinalDesiredCapturePointPosition(FramePoint finalDesiredCapturePointPositionToPack) { if (isStanding.getBooleanValue()) referenceCMPsCalculator.getNextEntryCMP(tempFinalICP); else entryCornerPoints.get(1).getFrameTupleIncludingFrame(tempFinalICP); tempFinalICP.changeFrame(worldFrame); finalDesiredCapturePointPositionToPack.setIncludingFrame(tempFinalICP); }
public void convertToFramePose(FramePoint endEffectorPositionIn, FrameOrientation endEffectorOrientationIn, FramePose poseToPack) { endEffectorPosition.setIncludingFrame(endEffectorPositionIn); endEffectorPosition.changeFrame(endEffectorFrame); endEffectorOrientation.setIncludingFrame(endEffectorOrientationIn); endEffectorOrientation.changeFrame(endEffectorFrame); poseToPack.setPoseIncludingFrame(endEffectorPosition, endEffectorOrientation); }
public void set(GeometricJacobian geometricJacobian, FramePoint point) { if (geometricJacobian.getBaseFrame() != geometricJacobian.getJacobianFrame()) throw new RuntimeException("Jacobian must be expressed in base frame"); this.geometricJacobian = geometricJacobian; this.point.setIncludingFrame(point); this.point.changeFrame(geometricJacobian.getBaseFrame()); this.jacobianMatrix.reshape(3, geometricJacobian.getNumberOfColumns()); }
private void updateRootJointPosition(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, FramePoint centerOfMassPosition) { tempCenterOfMassPositionState.setIncludingFrame(centerOfMassPosition); estimationFrame.getTransformToDesiredFrame(worldFrame).getRotation(tempOrientationStateReconstructMatrix); tempOrientationStateReconstruct.set(tempOrientationStateReconstructMatrix); computeEstimationLinkToWorldTransform(estimationFrame, tempEstimationLinkToWorld, tempCenterOfMassPositionState, tempOrientationStateReconstruct); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); rootJoint.setPositionAndRotation(tempRootJointToWorld); }
private void updateRootJointConfiguration(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame) { tempCenterOfMassPositionState.setIncludingFrame(centerOfMassPositionPort.getData()); tempOrientationState.setIncludingFrame(orientationPort.getData()); computeEstimationLinkTransform(estimationFrame, tempEstimationLinkToWorld, tempCenterOfMassPositionState, tempOrientationState); computeRootJointTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); rootJoint.setPositionAndRotation(tempRootJointToWorld); }
public void startComputation() { kinematicPoint.getVelocity(pointVelocity); yoFrameVectorPerfect.set(pointVelocity); corrupt(pointVelocity); yoFrameVectorNoisy.set(pointVelocity); measurementPointInBodyFrame.setIncludingFrame(bodyFrame, kinematicPoint.getOffsetCopy()); velocityOfMeasurementPointInWorldFrame.setIncludingFrame(ReferenceFrame.getWorldFrame(), pointVelocity); boolean isPointVelocityValid = true; pointVelocityDataObject.set(rigidBody, measurementPointInBodyFrame, velocityOfMeasurementPointInWorldFrame, isPointVelocityValid); pointVelocityOutputPort.setData(pointVelocityDataObject); }
public void update() { toolBody.getInertia().setMass(objectMass.getDoubleValue()); temporaryPoint.setIncludingFrame(objectCenterOfMass.getFrameTuple()); temporaryPoint.changeFrame(elevatorFrame); toolFrame.setPositionAndUpdate(temporaryPoint); // Visualization toolFramePoint.setToZero(toolFrame); toolFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); objectCenterOfMassInWorld.set(toolFramePoint); }
public DenseMatrix64F computeResidual() { PointPositionDataObject data = pointPositionMeasurementInputPort.getData(); ReferenceFrame referenceFrame = referenceFrameMap.getFrameByName(pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, data.getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); residualVector.set(data.getMeasurementPointInWorldFrame()); residualVector.sub(tempFramePoint.getPoint()); MatrixTools.insertTuple3dIntoEJMLVector(residualVector, residual, 0); return residual; }