@Override public void computeAchievedAcceleration() { spatialAccelerationCalculator.getRelativeAcceleration(endEffectorAchievedAcceleration, base, endEffector); endEffectorAchievedAcceleration.getAngularPart(achievedAngularAcceleration); yoAchievedAngularAcceleration.setAndMatchFrame(achievedAngularAcceleration); }
@Override public void calculateJointDesiredChecksum(GenericCRC32 checksum) { checksum.update(jointAccelerationDesired.getAngularPart()); checksum.update(jointAccelerationDesired.getLinearPart()); }
@Override public void setQddDesired(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); jointAccelerationDesired.setAngularPart(sixDoFOriginalJoint.jointAccelerationDesired.getAngularPart()); jointAccelerationDesired.setLinearPart(sixDoFOriginalJoint.jointAccelerationDesired.getLinearPart()); }
@Override public void setQddDesired(InverseDynamicsJoint originalJoint) { PlanarJoint floatingJoint = checkAndGetAsPlanarJoint(originalJoint); jointAccelerationDesired.setAngularPart(floatingJoint.jointAccelerationDesired.getAngularPart()); jointAccelerationDesired.setLinearPart(floatingJoint.jointAccelerationDesired.getLinearPart()); }
public void computeAchievedAcceleration() { spatialAccelerationCalculator.getRelativeAcceleration(endEffectorAchievedAcceleration, base, endEffector); endEffectorAchievedAcceleration.getAngularPart(achievedAngularAcceleration); endEffectorAchievedAcceleration.getLinearPart(achievedLinearAcceleration); yoAchievedAngularAcceleration.setAndMatchFrame(achievedAngularAcceleration); yoAchievedLinearAcceleration.setAndMatchFrame(achievedLinearAcceleration); }
@Override public void calculateJointStateChecksum(GenericCRC32 checksum) { checksum.update(jointTranslation); checksum.update(jointRotation); checksum.update(jointTwist.getAngularPart()); checksum.update(jointTwist.getLinearPart()); checksum.update(jointAcceleration.getAngularPart()); checksum.update(jointAcceleration.getLinearPart()); }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); setPosition(sixDoFOriginalJoint.jointTranslation); setRotation(sixDoFOriginalJoint.jointRotation); jointTwist.setAngularPart(sixDoFOriginalJoint.jointTwist.getAngularPart()); jointTwist.setLinearPart(sixDoFOriginalJoint.jointTwist.getLinearPart()); jointAcceleration.setAngularPart(sixDoFOriginalJoint.jointAcceleration.getAngularPart()); jointAcceleration.setLinearPart(sixDoFOriginalJoint.jointAcceleration.getLinearPart()); }
private void updateOrientationVisualization() { desiredSpatialAcceleration.getAngularPart(desiredAngularAcceleration); yoDesiredAngularAcceleration.setAndMatchFrame(desiredAngularAcceleration); tempOrientation.setToZero(accelerationControlModule.getTrackingFrame()); yoCurrentOrientation.setAndMatchFrame(tempOrientation); accelerationControlModule.getEndEffectorCurrentAngularVelocity(tempAngularVelocity); yoCurrentAngularVelocity.setAndMatchFrame(tempAngularVelocity); yoDesiredOrientation.get(tempAxisAngle); yoDesiredRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ()); yoDesiredRotationVector.scale(tempAxisAngle.getAngle()); yoCurrentOrientation.get(tempAxisAngle); yoCurrentRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ()); yoCurrentRotationVector.scale(tempAxisAngle.getAngle()); }
/** * This method provides a spatial acceleration controller. * @param spatialAccelerationToPack spatial acceleration to return. * @param desiredPose desired pose that we want to achieve. * @param desiredTwist desired twist to damp around. * @param feedForwardAcceleration feed forward acceleration from a reference trajectory. * @param currentTwist current twist of the rigid body. */ public void compute(SpatialAccelerationVector spatialAccelerationToPack, FramePose desiredPose, Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkBodyFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkBaseFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkExpressedInFrames(desiredTwist, feedForwardAcceleration, currentTwist); spatialAccelerationToPack.setToZero(bodyFrame, feedForwardAcceleration.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); feedForwardAcceleration.getAngularPart(feedForwardAngularAction); currentTwist.getAngularPart(currentAngularVelocity); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAction); spatialAccelerationToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); feedForwardAcceleration.getLinearPart(feedForwardLinearAction); currentTwist.getLinearPart(currentVelocity); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, currentVelocity, feedForwardLinearAction); spatialAccelerationToPack.setLinearPart(actionFromPositionController.getVector()); }
private void transformDesiredsFromSoleFrameToControlFrame() { desiredSoleFrame.setPoseAndUpdate(desiredPosition, desiredOrientation); // change pose desiredPose.setToZero(desiredControlFrame); desiredPose.changeFrame(worldFrame); desiredPose.getPosition(desiredPosition.getPoint()); desiredPose.getOrientation(desiredOrientation.getQuaternion()); // change twist desiredLinearVelocity.changeFrame(desiredSoleFrame); desiredAngularVelocity.changeFrame(desiredSoleFrame); desiredTwist.set(desiredSoleFrame, worldFrame, desiredSoleFrame, desiredLinearVelocity, desiredAngularVelocity); desiredTwist.changeFrame(desiredControlFrame); desiredTwist.getLinearPart(desiredLinearVelocity); desiredTwist.getAngularPart(desiredAngularVelocity); desiredLinearVelocity.changeFrame(worldFrame); desiredAngularVelocity.changeFrame(worldFrame); // change spatial acceleration desiredLinearAcceleration.changeFrame(desiredSoleFrame); desiredAngularAcceleration.changeFrame(desiredSoleFrame); desiredSpatialAcceleration.set(desiredSoleFrame, worldFrame, desiredSoleFrame, desiredLinearAcceleration, desiredAngularAcceleration); desiredSpatialAcceleration.changeFrameNoRelativeMotion(desiredControlFrame); desiredSpatialAcceleration.getLinearPart(desiredLinearAcceleration); desiredSpatialAcceleration.getAngularPart(desiredAngularAcceleration); desiredLinearAcceleration.changeFrame(worldFrame); desiredAngularAcceleration.changeFrame(worldFrame); }
spatialAccelerationOfMeasurementLink.getAngularPart(omegad); MatrixTools.toTildeForm(omegadTilde, omegad);
public void computeDynamicWrenchInBodyCoordinates(SpatialAccelerationVector acceleration, Twist twist, Wrench dynamicWrenchToPack) // TODO: write test { checkExpressedInBodyFixedFrame(); checkIsCrossPartZero(); // otherwise this operation would be a lot less efficient acceleration.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); acceleration.getBaseFrame().checkIsWorldFrame(); acceleration.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); twist.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); twist.getBaseFrame().checkIsWorldFrame(); twist.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); dynamicWrenchToPack.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); dynamicWrenchToPack.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); massMomentOfInertiaPart.transform(acceleration.getAngularPart(), tempVector); // J * omegad dynamicWrenchToPack.setAngularPart(tempVector); // [J * omegad; 0] massMomentOfInertiaPart.transform(twist.getAngularPart(), tempVector); // J * omega tempVector.cross(twist.getAngularPart(), tempVector); // omega x J * omega dynamicWrenchToPack.addAngularPart(tempVector); // [J * omegad + omega x J * omega; 0] tempVector.set(acceleration.getLinearPart()); // vd tempVector.scale(mass); // m * vd dynamicWrenchToPack.setLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd] tempVector.set(twist.getLinearPart()); // v tempVector.scale(mass); // m * v tempVector.cross(twist.getAngularPart(), tempVector); // omega x m * v dynamicWrenchToPack.addLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd + omega x m * v] }
tempRootToEstimationAcceleration.getAngularPart(tempRootToEstimationAngularAcceleration); tempRootToEstimationAngularAcceleration.changeFrame(estimationFrame);
spatialAccelerationOfMeasurementLink.getAngularPart(tempVector); MatrixTools.toTildeForm(tempMatrix, tempVector); phiJOmegad.mul(tempMatrix, rotationFromEstimationToWorld); spatialAccelerationOfMeasurementLink.getAngularPart(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); s.add(tempFrameVector);