public void getAngularVelocity(Vector3d angularVelocityToPack) { angularVelocityToPack.set(0.0, jointTwist.getAngularPartY(), 0.0); }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { matrix.set(rowStart + 0, 0, jointTwist.getAngularPartY()); matrix.set(rowStart + 1, 0, jointTwist.getLinearPartX()); matrix.set(rowStart + 2, 0, jointTwist.getLinearPartZ()); }
protected void getYoValuesFromTwist() { linearPart.set(twist.getExpressedInFrame(), twist.getLinearPartX(), twist.getLinearPartY(), twist.getLinearPartZ()); angularPart.set(twist.getExpressedInFrame(), twist.getAngularPartX(), twist.getAngularPartY(), twist.getAngularPartZ()); }
private void computeDesiredsForFreeMotion() { boolean blockToMaximumPitch = tempYawPitchRoll[1] > maximumToeOffAngleProvider.getValue(); if (blockToMaximumPitch) { toeOffDesiredPitchAngle.set(maximumToeOffAngleProvider.getValue()); toeOffDesiredPitchVelocity.set(0.0); } else { toeOffDesiredPitchAngle.set(desiredOrientation.getPitch()); toeOffDesiredPitchVelocity.set(footTwist.getAngularPartY()); } toeOffDesiredPitchAcceleration.set(0.0); }
@Override public void getJointTwist(Twist twistToPack) { twistToPack.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); twistToPack.setAngularPartY(jointTwist.getAngularPartY()); twistToPack.setLinearPartX(jointTwist.getLinearPartX()); twistToPack.setLinearPartZ(jointTwist.getLinearPartZ()); }
public void setJointTwist(Twist jointTwist) { this.jointTwist.checkReferenceFramesMatch(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); this.jointTwist.setAngularPartY(jointTwist.getAngularPartY()); this.jointTwist.setLinearPartX(jointTwist.getLinearPartX()); this.jointTwist.setLinearPartZ(jointTwist.getLinearPartZ()); }
@Override public void calculateJointStateChecksum(GenericCRC32 checksum) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); checksum.update(yawPitchRoll[1]); checksum.update(jointTwist.getAngularPartY()); checksum.update(jointAcceleration.getAngularPartY()); checksum.update(jointTranslation.getX()); checksum.update(jointTranslation.getY()); checksum.update(jointTwist.getLinearPartX()); checksum.update(jointTwist.getLinearPartZ()); checksum.update(jointAcceleration.getLinearPartX()); checksum.update(jointAcceleration.getLinearPartZ()); }
@Override public void doSpecificAction() { feedbackControlCommandList.clear(); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getYawPitchRoll(tempYawPitchRoll); twistCalculator.getRelativeTwist(footTwist, rootBody, contactableFoot.getRigidBody()); footTwist.changeFrame(contactableFoot.getFrameAfterParentJoint()); toeOffCurrentPitchAngle.set(tempYawPitchRoll[1]); toeOffCurrentPitchVelocity.set(footTwist.getAngularPartY()); desiredPosition.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredPosition.changeFrame(worldFrame); computeDesiredsForFreeMotion(); desiredOrientation.setYawPitchRoll(desiredYawToHold, toeOffDesiredPitchAngle.getDoubleValue(), desiredRollToHold); desiredLinearVelocity.setToZero(worldFrame); desiredAngularVelocity.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchVelocity.getDoubleValue(), 0.0); desiredAngularVelocity.changeFrame(worldFrame); desiredLinearAcceleration.setToZero(worldFrame); desiredAngularAcceleration.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchAcceleration.getDoubleValue(), 0.0); desiredAngularAcceleration.changeFrame(worldFrame); orientationFeedbackControlCommand.set(desiredOrientation, desiredAngularVelocity, desiredAngularAcceleration); pointFeedbackControlCommand.set(desiredContactPointPosition, desiredLinearVelocity, desiredLinearAcceleration); setupSingleContactPoint(); feedbackControlCommandList.addCommand(orientationFeedbackControlCommand); feedbackControlCommandList.addCommand(pointFeedbackControlCommand); }