@Override public void getDesiredPredecessorAcceleration(SpatialAccelerationVector accelerationToPack) { getDesiredJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.invert(); accelerationToPack.changeFrameNoRelativeMotion(predecessorFrame); }
@Override public void setQddDesired(InverseDynamicsJoint originalJoint) { SixDoFJoint sixDoFOriginalJoint = checkAndGetAsSiXDoFJoint(originalJoint); jointAccelerationDesired.setAngularPart(sixDoFOriginalJoint.jointAccelerationDesired.getAngularPart()); jointAccelerationDesired.setLinearPart(sixDoFOriginalJoint.jointAccelerationDesired.getLinearPart()); }
@Override public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart) { jointAccelerationDesired.set(jointAccelerationDesired.getBodyFrame(), jointAccelerationDesired.getBaseFrame(), jointAccelerationDesired.getExpressedInFrame(), matrix, rowStart); }
@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.setToZero(jointAcceleration.getBodyFrame(), jointAcceleration.getBaseFrame(), jointAcceleration.getExpressedInFrame()); accelerationToPack.setAngularPartY(jointAcceleration.getAngularPartY()); accelerationToPack.setLinearPartX(jointAcceleration.getLinearPartX()); accelerationToPack.setLinearPartZ(jointAcceleration.getLinearPartZ()); }
private void computeSuccessorAcceleration(InverseDynamicsJoint joint) { RigidBody predecessor = joint.getPredecessor(); RigidBody successor = joint.getSuccessor(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); joint.getPredecessorTwist(tempJointTwist); if (!doVelocityTerms) tempJointTwist.setToZero(); if (useDesireds) joint.getDesiredSuccessorAcceleration(tempJointAcceleration); else joint.getSuccessorAcceleration(tempJointAcceleration); if (!doAccelerationTerms) tempJointAcceleration.setToZero(); twistCalculator.getTwistOfBody(tempTwistFromWorld, predecessor); if (!doVelocityTerms) tempTwistFromWorld.setToZero(); SpatialAccelerationVector successorAcceleration = accelerations.get(successor); successorAcceleration.set(accelerations.get(predecessor)); successorAcceleration.changeFrame(successorFrame, tempJointTwist, tempTwistFromWorld); successorAcceleration.add(tempJointAcceleration); }
/** * 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); }
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] }
@Override public void doSpecificAction() { if (partialFootholdControlModule != null) { footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop); YoPlaneContactState contactState = momentumBasedController.getContactState(contactableFoot); boolean contactStateHasChanged = partialFootholdControlModule.applyShrunkPolygon(contactState); if (contactStateHasChanged) contactState.notifyContactStateHasChanged(); } footAcceleration.setToZero(contactableFoot.getFrameAfterParentJoint(), rootBody.getBodyFixedFrame(), contactableFoot.getFrameAfterParentJoint()); ReferenceFrame bodyFixedFrame = contactableFoot.getRigidBody().getBodyFixedFrame(); footAcceleration.changeBodyFrameNoRelativeAcceleration(bodyFixedFrame); footAcceleration.changeFrameNoRelativeMotion(bodyFixedFrame); spatialAccelerationCommand.setSpatialAcceleration(footAcceleration); }
public void setLinearAcceleration(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearAcceleration) { spatialAcceleration.setToZero(bodyFrame, baseFrame, desiredLinearAcceleration.getReferenceFrame()); spatialAcceleration.setLinearPart(desiredLinearAcceleration.getVector()); spatialAcceleration.changeFrameNoRelativeMotion(bodyFrame); }
twistCalculator.getRelativeTwist(twistOfMeasurementLink, elevator, measurementLink); spatialAccelerationCalculator.getRelativeAcceleration(spatialAccelerationOfMeasurementLink, elevator, measurementLink); spatialAccelerationOfMeasurementLink.changeFrame(elevatorFrame, twistOfMeasurementLink, twistOfMeasurementLink); spatialAccelerationOfMeasurementLink.getAngularPart(tempVector); MatrixTools.toTildeForm(tempMatrix, tempVector); phiJOmegad.mul(tempMatrix, rotationFromEstimationToWorld); twistOfMeasurementWithRespectToEstimation.changeFrame(measurementLink.getBodyFixedFrame()); twistOfMeasurementLink.changeFrame(measurementLink.getBodyFixedFrame()); spatialAccelerationOfMeasurementLink.changeFrame(estimationLink.getBodyFixedFrame(), twistOfMeasurementFrameWithRespectToEstimation, twistOfMeasurementLink); spatialAccelerationOfMeasurementLink.changeFrameNoRelativeMotion(estimationFrame); twistOfMeasurementLink.changeFrame(estimationFrame); spatialAccelerationOfMeasurementLink.getAngularPart(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); s.add(tempFrameVector); spatialAccelerationOfMeasurementLink.getLinearPart(tempFrameVector); s.add(tempFrameVector);
public void control(SpatialAccelerationVector handSpatialAccelerationVector, Wrench toolWrench) { if (!hasBeenInitialized) { update(); initialize(); } update(); toolAcceleration.set(handSpatialAccelerationVector); toolAcceleration.changeFrameNoRelativeMotion(toolJoint.getFrameAfterJoint()); // TODO: Take relative acceleration between uTorsoCoM and elevator in account toolAcceleration.changeBaseFrameNoRelativeAcceleration(elevatorFrame); toolAcceleration.changeBodyFrameNoRelativeAcceleration(toolJoint.getFrameAfterJoint()); toolJoint.setDesiredAcceleration(toolAcceleration); inverseDynamicsCalculator.compute(); inverseDynamicsCalculator.getJointWrench(toolJoint, toolWrench); toolWrench.negate(); toolWrench.changeFrame(handFixedFrame); toolWrench.changeBodyFrameAttachedToSameBody(handFixedFrame); // Visualization temporaryVector.setIncludingFrame(handFixedFrame, toolWrench.getLinearPartX(), toolWrench.getLinearPartY(), toolWrench.getLinearPartZ()); temporaryVector.changeFrame(ReferenceFrame.getWorldFrame()); temporaryVector.scale(0.01); objectForceInWorld.set(temporaryVector); }
@Override public void calculateJointDesiredChecksum(GenericCRC32 checksum) { checksum.update(jointAccelerationDesired.getAngularPart()); checksum.update(jointAccelerationDesired.getLinearPart()); }
getAccelerationOfBody(endEffectorAcceleration, body); endEffectorAcceleration.changeFrame(baseAcceleration.getBodyFrame(), twistOfCurrentWithRespectToNew, twistOfBodyWithRespectToBase); endEffectorAcceleration.sub(baseAcceleration); bodyFixedPoint.changeFrame(endEffectorAcceleration.getExpressedInFrame()); twistOfCurrentWithRespectToNew.changeFrame(endEffectorAcceleration.getExpressedInFrame()); endEffectorAcceleration.getAccelerationOfPointFixedInBodyFrame(twistOfCurrentWithRespectToNew, bodyFixedPoint, linearAccelerationToPack);
@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.set(jointAcceleration); }
@Override public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { getJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.changeFrameNoRelativeMotion(successorFrame); }
@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame); accelerationToPack.setAngularPart(jointAngularAcceleration.getVector()); }
@Override public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart) { jointAccelerationDesired.setToZero(); jointAccelerationDesired.setAngularPartY(matrix.get(rowStart + 0, 0)); jointAccelerationDesired.setLinearPartX(matrix.get(rowStart + 1, 0)); jointAccelerationDesired.setLinearPartZ(matrix.get(rowStart + 2, 0)); }
public static void setRandomAcceleration(SixDoFJoint rootJoint, Random random) { SpatialAccelerationVector jointAcceleration = new SpatialAccelerationVector(); rootJoint.getJointAcceleration(jointAcceleration); jointAcceleration.setAngularPart(RandomTools.generateRandomVector(random)); jointAcceleration.setLinearPart(RandomTools.generateRandomVector(random)); rootJoint.setAcceleration(jointAcceleration); }