private void computeProportionalTerm(FrameOrientation desiredOrientation) { desiredOrientation.changeFrame(bodyFrame); desiredOrientation.getQuaternion(errorQuaternion); errorAngleAxis.set(errorQuaternion); errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle())); // Limit the maximum position error considered for control action double maximumError = gains.getMaximumProportionalError(); if (errorAngleAxis.getAngle() > maximumError) { errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError); } proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ()); proportionalTerm.scale(errorAngleAxis.getAngle()); rotationErrorInBody.set(proportionalTerm); proportionalGainMatrix.transform(proportionalTerm.getVector()); }
public void setDesiredConfiguration(FrameOrientation orientation, FramePoint position) { desiredConfiguration.reshape(7, 1); MatrixTools.insertQuat4dIntoEJMLVector(desiredConfiguration, orientation.getQuaternion(), 0); MatrixTools.insertTuple3dIntoEJMLVector(position.getPoint(), desiredConfiguration, 4); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetters() { double epsilon = 1.0e-20; Random random = new Random(21651016L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame expectedFrame = worldFrame; double expectedTime = 0.0; FrameOrientation expectedOrientation = new FrameOrientation(expectedFrame); FrameVector expectedAngularVelocity = new FrameVector(expectedFrame); String expectedNamePrefix = "test"; String expectedNameSuffix = "blop"; YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint = new YoFrameSO3TrajectoryPoint( expectedNamePrefix, expectedNameSuffix, new YoVariableRegistry("schnoop"), expectedFrame); assertWaypointContainsExpectedData( expectedNamePrefix, expectedNameSuffix, expectedFrame, expectedTime, expectedOrientation, expectedAngularVelocity, testedYoFrameSO3TrajectoryPoint, epsilon); expectedFrame = worldFrame; expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0); expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame); expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame); testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity); assertWaypointContainsExpectedData( expectedNamePrefix, expectedNameSuffix, expectedFrame, expectedTime, expectedOrientation, expectedAngularVelocity, testedYoFrameSO3TrajectoryPoint, epsilon); expectedFrame = worldFrame; expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0); expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame); expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame); testedYoFrameSO3TrajectoryPoint.set( expectedTime, expectedOrientation.getQuaternion(), expectedAngularVelocity.getVector()); assertWaypointContainsExpectedData( expectedNamePrefix, expectedNameSuffix, expectedFrame, expectedTime, expectedOrientation, expectedAngularVelocity, testedYoFrameSO3TrajectoryPoint, epsilon); expectedFrame = worldFrame; expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0); expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, expectedFrame); expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, expectedFrame); YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint = new YoFrameSO3TrajectoryPoint( "sdfsd", "asd", new YoVariableRegistry("asawe"), expectedFrame); testedYoFrameSO3TrajectoryPoint.set(expectedYoFrameSO3TrajectoryPoint); assertTrue( expectedYoFrameSO3TrajectoryPoint.epsilonEquals(testedYoFrameSO3TrajectoryPoint, epsilon)); assertWaypointContainsExpectedData( expectedNamePrefix, expectedNameSuffix, testedYoFrameSO3TrajectoryPoint.getReferenceFrame(), testedYoFrameSO3TrajectoryPoint.getTime(), testedYoFrameSO3TrajectoryPoint.getOrientation().getFrameOrientationCopy(), testedYoFrameSO3TrajectoryPoint.getAngularVelocity().getFrameVectorCopy(), testedYoFrameSO3TrajectoryPoint, epsilon); }