public void setJointAngularVelocity(FrameVector jointAngularVelocity) { this.jointAngularVelocity.set(jointAngularVelocity); }
public void updateTranslation(FrameTuple<?, ?> frameVector) { originVector.set(frameVector); this.update(); }
public void updateTranslation(Tuple3d translation) { originVector.set(translation); this.update(); } }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { SphericalJoint originalSphericalJoint = checkAndGetAsSphericalJoint(originalJoint); setRotation(originalSphericalJoint.jointRotation); jointAngularVelocity.set(originalSphericalJoint.jointAngularVelocity.getVector()); jointAngularAcceleration.set(originalSphericalJoint.jointAngularAcceleration.getVector()); }
@Override public void setQddDesired(InverseDynamicsJoint originalJoint) { SphericalJoint originalSphericalJoint = checkAndGetAsSphericalJoint(originalJoint); jointAngularAccelerationDesired.set(originalSphericalJoint.jointAngularAccelerationDesired.getVector()); }
public void computeNextTick(FramePoint positionToPack, FrameVector velocityToPack, FrameVector accelerationToPack, double deltaT) { double time = this.time.getDoubleValue(); positionToPack.set(trajectory.getPosition(time)); velocityToPack.set(trajectory.getVelocity(time)); accelerationToPack.set(trajectory.getAcceleration(time)); }
public void propagateState(double dt) { if (centerOfMassAccelerationPort != null) { centerOfMassVelocityDelta.set(centerOfMassAccelerationPort.getData()); centerOfMassVelocityDelta.scale(dt); updateCenterOfMassVelocity(centerOfMassVelocityDelta); } }
public void propagateState(double dt) { centerOfMassVelocityDelta.set(centerOfMassAccelerationPort.getData()); centerOfMassVelocityDelta.scale(dt); updateCenterOfMassVelocity(centerOfMassVelocityDelta); }
private void updateCenterOfMassVelocity(FrameVector centerOfMassVelocityDelta) { centerOfMassVelocity.set(centerOfMassVelocityPort.getData()); centerOfMassVelocity.add(centerOfMassVelocityDelta); centerOfMassVelocityPort.setData(centerOfMassVelocity); } }
private void updateBias(FrameVector biasDelta) { bias.set(biasPort.getData()); bias.add(biasDelta); biasPort.setData(bias); } }
/** * Packs an existing FrameVector with the linear part. * The FrameVector must be in the same frame as the expressedInFrame */ public void getLinearPart(FrameVector vectorToPack) { this.getExpressedInFrame().checkReferenceFrameMatch(vectorToPack.getReferenceFrame()); vectorToPack.set(this.linearPart); }
public void getNormal(FrameVector normalToPack) { checkReferenceFrameMatch(normalToPack.getReferenceFrame()); this.plane3d.getNormal(temporaryVector); normalToPack.set(temporaryVector); }
/** * Packs an existing FrameVector with the angular part. * The FrameVector must be in the same frame as the expressedInFrame */ public void getAngularPart(FrameVector vectorToPack) { this.getExpressedInFrame().checkReferenceFrameMatch(vectorToPack.getReferenceFrame()); vectorToPack.set(this.angularPart); }
private void updateAngularVelocity(FrameVector angularVelocityDelta) { angularVelocity.set(angularVelocityPort.getData()); angularVelocity.add(angularVelocityDelta); angularVelocityPort.setData(angularVelocity); } }
public void propagateState(double dt) { FrameVector centerOfMassVelocity = centerOfMassVelocityPort.getData(); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPositionDelta.set(centerOfMassVelocity); centerOfMassPositionDelta.scale(dt); updateCenterOfMassPosition(centerOfMassPositionDelta); }
public void propagateState(double dt) { FrameVector centerOfMassVelocity = centerOfMassVelocityPort.getData(); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPositionDelta.set(centerOfMassVelocity); centerOfMassPositionDelta.scale(dt); updateCenterOfMassVelocity(centerOfMassPositionDelta); }
public void correctState(DenseMatrix64F correction) { MatrixTools.extractTuple3dFromEJMLVector(comAccelerationDelta.getVector(), correction, 0); comAcceleration.set(centerOfMassAccelerationStatePort.getData()); comAcceleration.add(comAccelerationDelta); centerOfMassAccelerationStatePort.setData(comAcceleration); } }
private void setProcessedSensors() { DenseMatrix64F xState = kalmanFilters.get(Direction.X).getState(); DenseMatrix64F yState = kalmanFilters.get(Direction.Y).getState(); DenseMatrix64F zState = kalmanFilters.get(Direction.Z).getState(); bodyPosition.set(xState.get(positionIndex), yState.get(positionIndex), zState.get(positionIndex)); bodyVelocity.set(xState.get(velocityIndex), yState.get(velocityIndex), zState.get(velocityIndex)); processedBodyPositionSensors.setBodyPosition(bodyPosition); processedBodyPositionSensors.setBodyVelocity(bodyVelocity); }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.getExpressedInFrame().checkReferenceFrameMatch(jointTorque.getReferenceFrame()); jointTorque.set(jointWrench.getAngularPart()); }