public void compute( FrameVector output, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity, FrameVector feedForward) { computeProportionalTerm(desiredOrientation); if (currentAngularVelocity != null) computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity); computeIntegralTerm(); output.setToZero(proportionalTerm.getReferenceFrame()); output.add(proportionalTerm); output.add(derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackAngularActionMagnitude = output.length(); double maximumAction = gains.getMaximumFeedback(); if (feedbackAngularActionMagnitude > maximumAction) { output.scale(maximumAction / feedbackAngularActionMagnitude); } feedbackAngularAction.set(output); rateLimitedFeedbackAngularAction.update(); rateLimitedFeedbackAngularAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
private void computeDerivativeTerm( FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) { desiredAngularVelocity.changeFrame(bodyFrame); currentAngularVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }
@ContinuousIntegrationTest(estimatedDuration = 0.2) @Test(timeout = 30000) public void testChangeFrame() throws Exception { double epsilon = 1.0e-10; Random random = new Random(21651016L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame expectedFrame = worldFrame; double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0); FrameOrientation expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame); FrameVector expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame); String expectedNamePrefix = "test"; String expectedNameSuffix = "blop"; YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint = new YoFrameSO3TrajectoryPoint( expectedNamePrefix, expectedNameSuffix, new YoVariableRegistry("schnoop"), expectedFrame); testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity); ReferenceFrame[] randomFrames = new ReferenceFrame[10]; randomFrames[0] = worldFrame; for (int i = 1; i < 10; i++) randomFrames[i] = ReferenceFrame.generateRandomReferenceFrame( "randomFrame" + i, random, random.nextBoolean() ? worldFrame : randomFrames[random.nextInt(i)]); for (int i = 0; i < 10000; i++) { expectedFrame = randomFrames[random.nextInt(10)]; testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame); expectedOrientation.changeFrame(expectedFrame); expectedAngularVelocity.changeFrame(expectedFrame); testedYoFrameSO3TrajectoryPoint.changeFrame(expectedFrame); assertWaypointContainsExpectedData( expectedNamePrefix, expectedNameSuffix, expectedFrame, expectedTime, expectedOrientation, expectedAngularVelocity, testedYoFrameSO3TrajectoryPoint, epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSingleRigidBodyRotation() { Random random = new Random(1766L); RigidBody elevator = new RigidBody("elevator", world); Vector3d jointAxis = RandomTools.generateRandomVector(random); jointAxis.normalize(); RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setIdentity(); RevoluteJoint joint = ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis); RigidBody body = ScrewTools.addRigidBody( "body", joint, RandomTools.generateRandomDiagonalMatrix3d(random), random.nextDouble(), new Vector3d()); joint.setQ(random.nextDouble()); joint.setQd(random.nextDouble()); Momentum momentum = computeMomentum(elevator, world); momentum.changeFrame(world); FrameVector linearMomentum = new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy()); FrameVector angularMomentum = new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy()); FrameVector linearMomentumCheck = new FrameVector(world); Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy(); Vector3d angularMomentumCheckVector = new Vector3d(jointAxis); angularMomentumCheckVector.scale(joint.getQd()); inertia.transform(angularMomentumCheckVector); FrameVector angularMomentumCheck = new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector); angularMomentumCheck.changeFrame(world); double epsilon = 1e-9; JUnitTools.assertTuple3dEquals( linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon); JUnitTools.assertTuple3dEquals( angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon); assertTrue(angularMomentum.length() > epsilon); }
protected List<FramePoint> getContactPoints(RobotSide swingSide) { double stepPitch = this.stepPitch.getDoubleValue(); List<FramePoint> allContactPoints = contactableBodies.get(swingSide).getContactPointsCopy(); if (stepPitch == 0.0) { return allContactPoints; } else { FrameVector forwardInFootFrame = new FrameVector(contactableBodies.get(swingSide).getFrameAfterParentJoint()); ReferenceFrame frame = allContactPoints.get(0).getReferenceFrame(); forwardInFootFrame.changeFrame(frame); forwardInFootFrame.scale(Math.signum(stepPitch)); int nPoints = 2; return DesiredFootstepCalculatorTools.computeMaximumPointsInDirection( allContactPoints, forwardInFootFrame, nPoints); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSomeMoreSettersAndGetters() { String namePrefix = "point"; String nameSuffix = "toTest"; YoVariableRegistry registry = new YoVariableRegistry("myRegistry"); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint = new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame); yoFrameSO3TrajectoryPoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); double time = 3.4; FrameOrientation orientation = new FrameOrientation(worldFrame, new Quat4d(0.1, 0.22, 0.34, 0.56)); FrameVector angularVelocity = new FrameVector(worldFrame, 1.7, 8.4, 2.2); yoFrameSO3TrajectoryPoint.setTime(time); yoFrameSO3TrajectoryPoint.setOrientation(orientation); yoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity); PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame)); FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2)); poseFrame.setPositionAndUpdate(poseFramePosition); FrameOrientation poseOrientation = new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2)); poseFrame.setOrientationAndUpdate(poseOrientation); yoFrameSO3TrajectoryPoint.registerReferenceFrame(poseFrame); yoFrameSO3TrajectoryPoint.changeFrame(poseFrame); assertFalse( orientation.epsilonEquals( yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10)); assertFalse( angularVelocity.epsilonEquals( yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10)); orientation.changeFrame(poseFrame); angularVelocity.changeFrame(poseFrame); assertTrue( orientation.epsilonEquals( yoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), 1e-10)); assertTrue( angularVelocity.epsilonEquals( yoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), 1e-10)); YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPointTwo = new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix + "Two", registry, poseFrame); yoFrameSO3TrajectoryPointTwo.setTime(time); yoFrameSO3TrajectoryPointTwo.setOrientation(orientation); yoFrameSO3TrajectoryPointTwo.setAngularVelocity(angularVelocity); assertTrue(yoFrameSO3TrajectoryPointTwo.epsilonEquals(yoFrameSO3TrajectoryPointTwo, 1e-10)); }