public void getFootToPelvisPosition(FramePoint positionToPack, RigidBody foot) { footToRootJointPositions.get(foot).getFrameTupleIncludingFrame(positionToPack); } }
@Override public void getLinearAccelerationBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector linearAccelerationBiasToPack) { Integer imuIndex = imuToIndexMap.get(imu); if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null) linearAccelerationBiasToPack.setToZero(imu.getMeasurementFrame()); else linearAccelerationBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(linearAccelerationBiasToPack); }
@Override public void getAngularVelocityBiasInIMUFrame(IMUSensorReadOnly imu, FrameVector angularVelocityBiasToPack) { Integer imuIndex = imuToIndexMap.get(imu); if (!enableIMUBiasCompensation.getBooleanValue() || imuIndex == null) angularVelocityBiasToPack.setToZero(imu.getMeasurementFrame()); else angularVelocityBiases.get(imuIndex.intValue()).getFrameTupleIncludingFrame(angularVelocityBiasToPack); }
private void updateTrustedFootPosition(RigidBody trustedFoot) { YoFramePoint footPositionInWorld = footPositionsInWorld.get(trustedFoot); AlphaFilteredYoFrameVector footToRootJointPosition = footToRootJointPositions.get(trustedFoot); if (trustCoPAsNonSlippingContactPoint.getBooleanValue()) { footToRootJointPosition.getFrameTupleIncludingFrame(tempPosition); rootJointPosition.getFrameTupleIncludingFrame(tempFramePoint); tempFramePoint.sub(tempPosition); // New foot position footPositionInWorld.getFrameTuple(tempPosition); // Previous foot position tempFrameVector.sub(tempFramePoint, tempPosition); // Delta from previous to new foot position copPositionsInWorld.get(trustedFoot).add(tempFrameVector); // New CoP position } footPositionInWorld.set(rootJointPosition); footPositionInWorld.sub(footToRootJointPosition); }
private void updateWristMeasuredWrench(FrameVector measuredForceToPack, FrameVector measuredTorqueToPack) { momentumBasedController.getWristMeasuredWrenchHandWeightCancelled(measuredWrench, robotSide); measuredWrench.getLinearPartIncludingFrame(tempForceVector); measuredWrench.getAngularPartIncludingFrame(tempTorqueVector); deadzoneMeasuredForce.update(tempForceVector); deadzoneMeasuredTorque.update(tempTorqueVector); filteredMeasuredForce.update(); filteredMeasuredTorque.update(); filteredMeasuredForce.getFrameTupleIncludingFrame(tempForceVector); filteredMeasuredTorque.getFrameTupleIncludingFrame(tempTorqueVector); measuredWrench.setLinearPart(tempForceVector); measuredWrench.setAngularPart(tempTorqueVector); measuredWrench.changeFrame(controlFrame); measuredWrench.getLinearPartIncludingFrame(measuredForceToPack); measuredWrench.getAngularPartIncludingFrame(measuredTorqueToPack); }