public static void setRandomPositionAndOrientation(FloatingInverseDynamicsJoint rootJoint, Random random) { rootJoint.setPositionAndRotation(RigidBodyTransform.generateRandomTransform(random)); }
private void readAndUpdateRootJointPositionAndOrientation() { packRootTransform(robot, temporaryRootToWorldTransform); temporaryRootToWorldTransform.normalizeRotationPart(); rootJoint.setPositionAndRotation(temporaryRootToWorldTransform); }
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); }
transform.setRotation(new Quat4d(q_qx.getDoubleValue(), q_qy.getDoubleValue(), q_qz.getDoubleValue(), q_qs.getDoubleValue())); fullRobotModel.getRootJoint().setPositionAndRotation(transform); fullRobotModel.updateFrames();
public void addNoise() { rootJointFrame.getTransformToParent(pelvisPose); updateBeforeYoVariables(); integrateError(); pelvisPose.getRotation(pelvisRotation); pelvisRotation.mul(rotationError); pelvisPose.setRotation(pelvisRotation); pelvisPose.getTranslation(pelvisTranslation); pelvisTranslation.add(translationError); pelvisPose.setTranslation(pelvisTranslation); updateAfterYoVariables(); rootJoint.setPositionAndRotation(pelvisPose); rootJointFrame.update(); }
@Override public void doControl(long timestamp) { if (pelvisPoseCorrectionCommunicator != null) { pelvisReferenceFrame.update(); checkForManualTrigger(); checkForNewPacket(); interpolationTranslationAlphaFilter.update(confidenceFactor.getDoubleValue()); interpolationRotationAlphaFilter.update(confidenceFactor.getDoubleValue()); pelvisReferenceFrame.getTransformToParent(pelvisPose); addPelvisePoseToPelvisBuffer(pelvisPose, timestamp); nonCorrectedPelvis.setAndUpdate(pelvisPose); correctPelvisPose(pelvisPose); correctedPelvis.setAndUpdate(pelvisPose); rootJoint.setPositionAndRotation(pelvisPose); pelvisReferenceFrame.update(); checkForNeedToSendCorrectionUpdate(); } }
private void updateCorrectedPelvis() { if(!hasOneIcpPacketEverBeenReceived.getBooleanValue()) correctedPelvisPoseInWorldFrame.setPose(stateEstimatorPelvisTransformInWorld); correctedPelvisPoseInWorldFrame.getPose(correctedPelvisTransformInWorldFrame); correctedPelvisTransformInWorldFrame.getTranslation(correctedPelvisTranslation); iterativeClosestPointInWorldFramePose.getPosition(localizationTranslation); iterativeClosestPointInWorldFramePose.getOrientationIncludingFrame(localizationOrientation); correctedPelvisOrientation.setIncludingFrame(worldFrame, correctedPelvisTransformInWorldFrame); errorBetweenCorrectedAndLocalizationTransform_Rotation.setOrientationFromOneToTwo(localizationOrientation, correctedPelvisOrientation); errorBetweenCorrectedAndLocalizationTransform_Rotation.getQuaternion(errorBetweenCorrectedAndLocalizationQuaternion_Rotation); errorBetweenCorrectedAndLocalizationTransform_Translation.sub(localizationTranslation, correctedPelvisTranslation); ////// for SCS feedback yoCorrectedPelvisPoseInWorldFrame.set(correctedPelvisPoseInWorldFrame); ////// errorBetweenCorrectedAndLocalizationTransform.setTranslation(errorBetweenCorrectedAndLocalizationTransform_Translation); errorBetweenCorrectedAndLocalizationTransform.setRotation(errorBetweenCorrectedAndLocalizationQuaternion_Rotation); rootJoint.setPositionAndRotation(correctedPelvisTransformInWorldFrame); }
sixDoFJoint.setPositionAndRotation(positionAndRotation);