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 rigidBody, Wrench externalWrench) { setRigidBody(rigidBody); externalWrenchAppliedOnRigidBody.set(externalWrench); externalWrenchAppliedOnRigidBody.changeFrame(rigidBody.getBodyFixedFrame()); }
public void set(RigidBody controlledBody, Wrench virtualWrench) { setRigidBody(controlledBody); virtualWrenchAppliedByRigidBody.set(virtualWrench); virtualWrenchAppliedByRigidBody.changeFrame(controlledBody.getBodyFixedFrame()); }
public Map<RigidBody, Wrench> computeWrenchesFromRho(DenseMatrix64F rho) { // Reinintialize wrenches for (int i = 0; i < rigidBodies.size(); i++) { RigidBody rigidBody = rigidBodies.get(i); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchesFromRho.get(rigidBody).setToZero(bodyFixedFrame, bodyFixedFrame); } int rhoStartIndex = 0; for (int i = 0; i < rigidBodies.size(); i++) { RigidBody rigidBody = rigidBodies.get(i); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); PlaneContactStateToWrenchMatrixHelper helper = planeContactStateToWrenchMatrixHelpers.get(rigidBody); helper.computeWrenchFromRho(rhoStartIndex, rho); Wrench wrenchFromRho = helper.getWrenchFromRho(); wrenchFromRho.changeFrame(bodyFixedFrame); wrenchesFromRho.get(rigidBody).add(wrenchFromRho); rhoStartIndex += helper.getRhoSize(); } return wrenchesFromRho; }
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 update() { for (int i = 0; i < allRigidBodies.size(); i++) { RigidBody rigidBody = allRigidBodies.get(i); FootSwitchInterface footSwitch = footSwitches.get(rigidBody); GeometricJacobian jacobian = jacobians.get(rigidBody); footSwitch.computeAndPackFootWrench(wrench); wrench.changeFrame(rigidBody.getBodyFixedFrame()); wrench.negate(); jacobian.compute(); jacobian.computeJointTorques(wrench, jointTorquesMatrix); InverseDynamicsJoint[] joints = jacobian.getJointsInOrder(); for (int j = 0; j < joints.length; j++) { OneDoFJoint joint = (OneDoFJoint) joints[j]; jointTorques.get(joint).set(jointTorquesMatrix.get(j, 0)); } } } }
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); } }
@Override public void doControl() { forceSensorData.getWrench(tempWrench); for(int i = 0; i < yoTorqueInJoints.size(); i++) { ImmutablePair<FrameVector, DoubleYoVariable> pair = yoTorqueInJoints.get(i); FrameVector jointAxis = pair.getLeft(); DoubleYoVariable torqueAboutJointAxis = pair.getRight(); tempWrench.changeFrame(jointAxis.getReferenceFrame()); tempFrameVector.setToZero(tempWrench.getExpressedInFrame()); tempWrench.getAngularPart(tempFrameVector); torqueAboutJointAxis.set(-tempFrameVector.dot(jointAxis)); } }
@Override public void doControl() { forceSensorData.getWrench(tempWrench); for(int i = 0; i < yoTorqueInJoints.size(); i++) { ImmutablePair<FrameVector, DoubleYoVariable> pair = yoTorqueInJoints.get(i); FrameVector jointAxis = pair.getLeft(); DoubleYoVariable torqueAboutJointAxis = pair.getRight(); tempWrench.changeFrame(jointAxis.getReferenceFrame()); tempFrameVector.setToZero(tempWrench.getExpressedInFrame()); tempWrench.getAngularPart(tempFrameVector); torqueAboutJointAxis.set(-tempFrameVector.dot(jointAxis)); } }
private void setFootMeasuredWrenches() { for (RobotSide robotSide : RobotSide.values) { WrenchBasedFootSwitch wrenchBasedFootSwitch = wrenchBasedFootSwitches.get(robotSide); wrenchBasedFootSwitch.computeAndPackFootWrench(tempWrench); RigidBody foot = wrenchBasedFootSwitch.getContactablePlaneBody().getRigidBody(); tempWrench.changeBodyFrameAttachedToSameBody(foot.getBodyFixedFrame()); tempWrench.changeFrame(foot.getBodyFixedFrame()); inverseDynamicsCalculator.setExternalWrench(foot, tempWrench); } }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = wristsubtreeCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempForce.setIncludingFrame(worldFrame, 0.0, 0.0, -wristSubtreeMass.get(robotSide).getDoubleValue() * gravity); tempForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = handCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempWristForce.setIncludingFrame(worldFrame, 0.0, 0.0, -handsMass.get(robotSide).getDoubleValue() * gravity); tempWristForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempWristForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }
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 void update(Wrench sensorWrench) { sensorWrench.changeFrame(world); yoSensorForce.set(sensorWrench.getExpressedInFrame(), sensorWrench.getLinearPartX(), sensorWrench.getLinearPartY(), sensorWrench.getLinearPartZ()); yoSensorTorque.set(sensorWrench.getExpressedInFrame(), sensorWrench.getAngularPartX(), sensorWrench.getAngularPartY(), sensorWrench.getAngularPartZ()); if (addSimulatedSensorNoise.getBooleanValue()) { double amp = 0.1; double bias = 0.25; yoSensorForce.add(amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias); yoSensorTorque.add(amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias, amp*2.0*(Math.random()-0.5)+bias); } updateSensorPosition(); updateCenterOfMass(); yoSensorToDistalCoMvectorInWorld.sub(distalCoMInWorld, yoSensorPositionInWorld); distalMassWrench.setToZero(world); distalMassWrench.setUsingArm(world, distalMassForceInWorld.getFrameTuple().getVector(), yoSensorToDistalCoMvectorInWorld.getFrameTuple().getVector()); yoSensorForceFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getLinearPartX(), distalMassWrench.getLinearPartY(), distalMassWrench.getLinearPartZ()); yoSensorTorqueFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getAngularPartX(), distalMassWrench.getAngularPartY(), distalMassWrench.getAngularPartZ()); yoSensorForceMassCompensated.sub(yoSensorForce, yoSensorForceFromDistalMass); yoSensorTorqueMassCompensated.sub(yoSensorTorque, yoSensorTorqueFromDistalMass); }
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()); }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.setToZero(successor.getBodyFixedFrame(), successorWrench.getExpressedInFrame()); jointWrench.setAngularPartY(successorWrench.getAngularPartY()); jointWrench.setLinearPartX(successorWrench.getLinearPartX()); jointWrench.setLinearPartZ(successorWrench.getLinearPartZ()); jointWrench.changeFrame(successor.getBodyFixedFrame()); }
private void updateWristMeasuredWrench(FrameVector measuredForceToPack, FrameVector measuredTorqueToPack) { momentumBasedController.getWristMeasuredWrenchHandWeightCancelled(measuredWrench, robotSide); measuredWrench.getLinearPartIncludingFrame(tempForceVector); measuredWrench.getAngularPartIncludingFrame(tempTorqueVector); deadzoneMeasuredForce.update(tempForceVector); deadzoneMeasuredTorque.update(tempTorqueVector); filteredMeasuredForce.update(); filteredMeasuredTorque.update(); filteredMeasuredForce.getFrameTupleIncludingFrame(tempForceVector); filteredMeasuredTorque.getFrameTupleIncludingFrame(tempTorqueVector); measuredWrench.setLinearPart(tempForceVector); measuredWrench.setAngularPart(tempTorqueVector); measuredWrench.changeFrame(controlFrame); measuredWrench.getLinearPartIncludingFrame(measuredForceToPack); measuredWrench.getAngularPartIncludingFrame(measuredTorqueToPack); }
tmpWrench.setAngularPart(tempFrameVector); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench);
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); }
private void readSensorData(Wrench footWrenchToPack) { forceSensorData.getWrench(footWrenchToPack); // First in measurement frame for all the frames... footForce.setToZero(footWrenchToPack.getExpressedInFrame()); footWrenchToPack.getLinearPart(footForce); yoFootForce.set(footForce); footTorque.setToZero(footWrenchToPack.getExpressedInFrame()); footWrenchToPack.getAngularPart(footTorque); yoFootTorque.set(footTorque); // magnitude of force part is independent of frame footForceMagnitude.set(footForce.length()); // Now change to frame after the parent joint (ankle or wrist for example): footWrenchInBodyFixedFrame.set(footWrenchToPack); footWrenchInBodyFixedFrame.changeFrame(contactablePlaneBody.getRigidBody().getBodyFixedFrame()); footForce.setToZero(footWrenchInBodyFixedFrame.getExpressedInFrame()); footWrenchInBodyFixedFrame.getLinearPart(footForce); footTorque.setToZero(footWrenchInBodyFixedFrame.getExpressedInFrame()); footWrenchInBodyFixedFrame.getAngularPart(footTorque); footForce.changeFrame(contactablePlaneBody.getFrameAfterParentJoint()); yoFootForceInFoot.set(footForce); footTorque.changeFrame(contactablePlaneBody.getFrameAfterParentJoint()); yoFootTorqueInFoot.set(footTorque); footForce.changeFrame(ReferenceFrame.getWorldFrame()); footTorque.changeFrame(ReferenceFrame.getWorldFrame()); yoFootForceInWorld.set(footForce); yoFootTorqueInWorld.set(footTorque); updateSensorVisualizer(); }