/** * Sets angular and linear parts using a Matrix ([torque; force]). * @param bodyFrame the frame/body on which the wrench is exerted * @param expressedInFrame the frame in which the wrench is expressed */ public void set(ReferenceFrame bodyFrame, ReferenceFrame expressedInFrame, DenseMatrix64F wrench) { set(expressedInFrame, wrench); this.bodyFrame = bodyFrame; }
public void computeTotalWrench(Wrench totalGroundReactionWrenchToPack, Collection<Wrench> wrenches, ReferenceFrame referenceFrame) { totalGroundReactionWrenchToPack.setToZero(referenceFrame, referenceFrame); for (Wrench wrench : wrenches) { temporaryWrench.set(wrench); temporaryWrench.changeFrame(referenceFrame); temporaryWrench.changeBodyFrameAttachedToSameBody(referenceFrame); totalGroundReactionWrenchToPack.add(temporaryWrench); } } }
public void computeWrenchFromRho(int startIndex, DenseMatrix64F allRobotRho) { CommonOps.extract(allRobotRho, startIndex, startIndex + rhoSize, 0, 1, rhoMatrix, 0, 0); yoRho.set(rhoMatrix); if (yoPlaneContactState.inContact()) { totalWrenchMatrix.zero(); for (int rhoIndex = 0; rhoIndex < rhoSize; rhoIndex++) { double rho = rhoMatrix.get(rhoIndex, 0); CommonOps.extract(rhoJacobianMatrix, 0, SpatialForceVector.SIZE, rhoIndex, rhoIndex + 1, singleRhoWrenchMatrix, 0, 0); MatrixTools.addMatrixBlock(totalWrenchMatrix, 0, 0, singleRhoWrenchMatrix, 0, 0, SpatialForceVector.SIZE, 1, rho); } RigidBody rigidBody = yoPlaneContactState.getRigidBody(); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchFromRho.set(bodyFixedFrame, centerOfMassFrame, totalWrenchMatrix); CommonOps.mult(copJacobianMatrix, rhoMatrix, previousCoPMatrix); previousCoP.setX(previousCoPMatrix.get(0, 0)); previousCoP.setY(previousCoPMatrix.get(1, 0)); } else { wrenchFromRho.setToZero(); } }
@Override public void set(ExternalWrenchCommand other) { rigidBody = other.rigidBody; rigidBodyName = other.rigidBodyName; externalWrenchAppliedOnRigidBody.set(other.externalWrenchAppliedOnRigidBody); }
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()); }
public void getWrench(Wrench wrenchToPack) { wrenchToPack.set(successorWrench); }
public void setExternalWrench(RigidBody rigidBody, Wrench externalWrench) { externalWrenches.get(rigidBody).set(externalWrench); }
public void getJointWrench(InverseDynamicsJoint joint, Wrench wrenchToPack) { wrenchToPack.set(jointWrenches.get(joint)); } }
public void getExternalWrench(RigidBody rigidBody, Wrench externalWrenchToPack) { externalWrenchToPack.set(externalWrenches.get(rigidBody)); }
@Override public void getWrench(Wrench wrenchToPack) { wrenchToPack.changeBodyFrameAttachedToSameBody(measurementFrame); wrenchToPack.set(measurementFrame, wrench); }
@Override public void set(VirtualWrenchCommand other) { controlledBody = other.controlledBody; rigidBodyName = other.rigidBodyName; virtualWrenchAppliedByRigidBody.set(other.virtualWrenchAppliedByRigidBody); selectionMatrix.set(other.selectionMatrix); }
public void set(RigidBody rigidBody, Wrench externalWrench) { setRigidBody(rigidBody); externalWrenchAppliedOnRigidBody.set(externalWrench); externalWrenchAppliedOnRigidBody.changeFrame(rigidBody.getBodyFixedFrame()); }
public Wrench computeTotalExternalWrench(ReferenceFrame referenceFrame) { Wrench totalGroundReactionWrench = new Wrench(referenceFrame, referenceFrame); Wrench temporaryWrench = new Wrench(); for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) { Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); temporaryWrench.set(externalWrench); temporaryWrench.changeFrame(referenceFrame); temporaryWrench.changeBodyFrameAttachedToSameBody(referenceFrame); totalGroundReactionWrench.add(temporaryWrench); } return totalGroundReactionWrench; }
public void set(RigidBody controlledBody, Wrench virtualWrench) { setRigidBody(controlledBody); virtualWrenchAppliedByRigidBody.set(virtualWrench); virtualWrenchAppliedByRigidBody.changeFrame(controlledBody.getBodyFixedFrame()); }
private void computeJointWrenchesAndTorques() { for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) { InverseDynamicsJoint joint = allJoints.get(jointIndex); RigidBody successor = joint.getSuccessor(); Wrench jointWrench = jointWrenches.get(joint); jointWrench.set(netWrenches.get(successor)); Wrench externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); List<InverseDynamicsJoint> childrenJoints = successor.getChildrenJoints(); for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) { InverseDynamicsJoint child = childrenJoints.get(childIndex); if (!jointsToIgnore.contains(child)) { Wrench wrenchExertedOnChild = jointWrenches.get(child); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); wrenchExertedByChild.set(wrenchExertedOnChild); wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame); wrenchExertedByChild.scale(-1.0); // Action = -reaction wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame()); jointWrench.sub(wrenchExertedByChild); } } joint.setTorqueFromWrench(jointWrench); } }
public void setExternalWrenchToCompensateFor(RigidBody rigidBody, Wrench wrench) { boolean containsRigidBody = externalWrenchesToCompensateFor.get(rigidBody) != null; if (!containsRigidBody) { externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); externalWrenchesToCompensateFor.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); externalWrenchesToCompensateForList.add(externalWrenchesToCompensateFor.get(rigidBody)); rigidBodiesWithWrenchToCompensateFor.add(rigidBody); } ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrench.getBodyFrame().checkReferenceFrameMatch(bodyFixedFrame); wrench.getExpressedInFrame().checkReferenceFrameMatch(bodyFixedFrame); externalWrenchesToCompensateFor.get(rigidBody).set(wrench); }
private void handleVirtualWrenchCommand(VirtualWrenchCommand command) { virtualWrenchCommandList.addCommand(command); if (command.getControlledBody() == controlRootBody) { tmpExternalWrench.set(command.getVirtualWrench()); tmpExternalWrench.negate(); optimizationControlModule.submitExternalWrench(command.getControlledBody(), tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }
private void handleExternalWrenchCommand(ExternalWrenchCommand command) { optimizationControlModule.submitExternalWrench(command.getRigidBody(), tmpExternalWrench); tmpWrench.set(command.getExternalWrench()); tmpWrench.negate(); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(command.getRigidBody(), tmpWrench); virtualWrenchCommandList.addCommand(virtualWrenchCommand); }
private void storeJointState() { ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); for (InverseDynamicsJoint joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.set(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); } }
public static void computeWrench(Wrench groundReactionWrenchToPack, FrameVector force, FramePoint2d cop, double normalTorque) { ReferenceFrame referenceFrame = cop.getReferenceFrame(); force.changeFrame(referenceFrame); // r x f + tauN Vector3d torque = new Vector3d(); torque.setX(cop.getY() * force.getZ()); torque.setY(-cop.getX() * force.getZ()); torque.setZ(cop.getX() * force.getY() - cop.getY() * force.getX() + normalTorque); groundReactionWrenchToPack.set(referenceFrame, force.getVector(), torque); }