private void checkExpressedInFrames(Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkExpressedInFrames(desiredTwist, currentTwist); feedForwardAcceleration.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); }
@Override public void setDesiredAcceleration(DenseMatrix64F matrix, int rowStart) { jointAccelerationDesired.set(jointAccelerationDesired.getBodyFrame(), jointAccelerationDesired.getBaseFrame(), jointAccelerationDesired.getExpressedInFrame(), matrix, rowStart); }
/** * Changes the spatial acceleration of the root. * Even though the root is assumed to be non-moving, the {@code rootAcceleration} is usually * set to the opposite of the gravitational acceleration, such the effect of the gravity is * naturally propagated to the entire system. * * @param newRootAcceleration the new spatial acceleration of the root. * @throws ReferenceFrameMismatchException if any of the reference frames of {@code newRootAcceleration} does * not match this calculator's root spatial acceleration's frames. */ public void setRootAcceleration(SpatialAccelerationVector newRootAcceleration) { rootAcceleration.checkReferenceFramesMatch(newRootAcceleration.getBodyFrame(), newRootAcceleration.getBaseFrame(), newRootAcceleration.getExpressedInFrame()); this.rootAcceleration.set(newRootAcceleration); }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); if (!jacobianSolver.setA(jacobian.getJacobianMatrix())) throw new RuntimeException("jacobian cannot be solved"); jacobianSolver.solve(biasedSpatialAcceleration, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); jacobianSolver.setJacobian(jacobian.getJacobianMatrix()); jacobianSolver.solve(jointAccelerations, biasedSpatialAcceleration); CommonOps.scale(sign, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
bodyFixedPoint.changeFrame(endEffectorAcceleration.getExpressedInFrame()); twistOfCurrentWithRespectToNew.changeFrame(endEffectorAcceleration.getExpressedInFrame()); endEffectorAcceleration.getAccelerationOfPointFixedInBodyFrame(twistOfCurrentWithRespectToNew, bodyFixedPoint, linearAccelerationToPack);
@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()); }
@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()); }
getAccelerationOfBody(spatialAccelerationToPack, body); baseAcceleration.changeFrame(spatialAccelerationToPack.getExpressedInFrame(), twistOfCurrentWithRespectToNew, twistOfBodyWithRespectToBase); spatialAccelerationToPack.sub(baseAcceleration);
private void handleSpatialAccelerationCommand(SpatialAccelerationCommand command) { RigidBody controlledBody = command.getEndEffector(); SpatialAccelerationVector accelerationVector = command.getSpatialAcceleration(); accelerationVector.changeBaseFrameNoRelativeAcceleration(ReferenceFrame.getWorldFrame()); twistCalculator.getTwistOfBody(tmpTwist, controlledBody); tmpWrench.setToZero(accelerationVector.getBodyFrame(), accelerationVector.getExpressedInFrame()); conversionInertias.get(controlledBody).computeDynamicWrenchInBodyCoordinates(accelerationVector, tmpTwist, tmpWrench); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(controlledBody, tmpWrench, command.getSelectionMatrix()); virtualWrenchCommandList.addCommand(virtualWrenchCommand); if (controlledBody == controlRootBody) { tmpExternalWrench.set(tmpWrench); tmpExternalWrench.negate(); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }
inertia.changeFrame(tempSpatialAcceleration.getExpressedInFrame()); // easier to change the frame of inertia than the spatial acceleration tempMomentum.set(tempSpatialAcceleration.getExpressedInFrame(), inertiaTimesSpatialAccel);
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] }
RigidBody base = commandToConvert.getBase(); RigidBody endEffector = commandToConvert.getEndEffector(); long jacobianId = geometricJacobianHolder.getOrCreateGeometricJacobian(base, endEffector, spatialAcceleration.getExpressedInFrame()); GeometricJacobian jacobian = geometricJacobianHolder.getJacobian(jacobianId);