@Override public void setDesiredAccelerationToZero() { jointAccelerationDesired.setToZero(); }
@Override public void setDesiredAccelerationToZero() { jointAccelerationDesired.setToZero(); }
@Override public void doAction() { handAcceleration.setToZero(handFrame, elevatorFrame, handFrame); spatialAccelerationCommand.setSpatialAcceleration(handAcceleration); }
@Override public void getDesiredJointAcceleration(SpatialAccelerationVector jointAcceleration) { jointAcceleration.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame); jointAcceleration.setAngularPart(jointAngularAccelerationDesired.getVector()); }
@Override public void getJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.setToZero(afterJointFrame, beforeJointFrame, afterJointFrame); accelerationToPack.setAngularPart(jointAngularAcceleration.getVector()); }
public void setAngularAcceleration(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredAngularAcceleration) { spatialAcceleration.setToZero(bodyFrame, baseFrame, desiredAngularAcceleration.getReferenceFrame()); spatialAcceleration.setAngularPart(desiredAngularAcceleration.getVector()); }
public void setLinearAcceleration(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearAcceleration) { spatialAcceleration.setToZero(bodyFrame, baseFrame, desiredLinearAcceleration.getReferenceFrame()); spatialAcceleration.setLinearPart(desiredLinearAcceleration.getVector()); spatialAcceleration.changeFrameNoRelativeMotion(bodyFrame); }
@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)); }
@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); }
@Override public void getDesiredJointAcceleration(SpatialAccelerationVector accelerationToPack) { accelerationToPack.setToZero(jointAccelerationDesired.getBodyFrame(), jointAccelerationDesired.getBaseFrame(), jointAccelerationDesired.getExpressedInFrame()); accelerationToPack.setAngularPartY(jointAccelerationDesired.getAngularPartY()); accelerationToPack.setLinearPartX(jointAccelerationDesired.getLinearPartX()); accelerationToPack.setLinearPartZ(jointAccelerationDesired.getLinearPartZ()); }
accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector(); InverseDynamicsJoint[] jointPathFromBaseToEndEffector = jacobian.getJointPathFromBaseToEndEffector(); zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist); zeroAcceleration.add(accelerationToPack);
public void computeJacobianDerivativeTerm(SpatialAccelerationVector accelerationToPack) { ReferenceFrame endEffectorFrame = jacobian.getEndEffectorFrame(); twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector(); for (int i = jacobian.getJointsInOrder().length - 1; i >= 0; i--) { twistOfCurrentWithRespectToBase.changeFrame(currentBody.getBodyFixedFrame()); InverseDynamicsJoint joint = jacobian.getJointsInOrder()[i]; if (currentBody == joint.getPredecessor()) { joint.getPredecessorTwist(jointTwist); currentBody = joint.getSuccessor(); } else { joint.getSuccessorTwist(jointTwist); currentBody = joint.getPredecessor(); } zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist); zeroAcceleration.add(accelerationToPack); accelerationToPack.set(zeroAcceleration); jointTwist.invert(); twistOfCurrentWithRespectToBase.add(jointTwist); } accelerationToPack.changeBodyFrameNoRelativeAcceleration(endEffectorFrame); }
@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); }
/** * 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()); }
originAcceleration.changeFrame(sixDoFJoint.getFrameBeforeJoint()); spatialAccelerationVector.setToZero(sixDoFJoint.getFrameAfterJoint(), sixDoFJoint.getFrameBeforeJoint(), sixDoFJoint.getFrameAfterJoint()); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist);
footAcceleration.setToZero(controlFrame, rootBody.getBodyFixedFrame(), controlFrame); ReferenceFrame bodyFixedFrame = contactableFoot.getRigidBody().getBodyFixedFrame(); footAcceleration.changeBodyFrameNoRelativeAcceleration(bodyFixedFrame);