@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)); } }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.getExpressedInFrame().checkReferenceFrameMatch(jointTorque.getReferenceFrame()); jointTorque.set(jointWrench.getAngularPart()); }
/** * Takes the dot product of this twist and a wrench, resulting in the (reference frame independent) instantaneous power. * @param wrench a wrench that * 1) has an 'onWhat' reference frame that is the same as this twist's 'bodyFrame' reference frame. * 2) is expressed in the same reference frame as this twist * @return the instantaneous power associated with this twist and the wrench */ public double dot(Wrench wrench) { this.bodyFrame.checkReferenceFrameMatch(wrench.getBodyFrame()); this.expressedInFrame.checkReferenceFrameMatch(wrench.getExpressedInFrame()); double power = this.angularPart.dot(wrench.getAngularPart()) + this.linearPart.dot(wrench.getLinearPart()); return power; }
tempWrench.getAngularPart(tempVector); tempVector.changeFrame(ReferenceFrame.getWorldFrame()); torque.set(tempVector);
private void updateFootSensorRawMeasurement() { for (RobotSide robotSide : RobotSide.values) { FootSwitchInterface footSwitch = footSwitches.get(robotSide); tempWrench.setToZero(footSwitch.getMeasurementFrame(), footSwitch.getMeasurementFrame()); tempFrameVector.setToZero(footSwitch.getMeasurementFrame()); footSwitch.computeAndPackFootWrench(tempWrench); tempWrench.getLinearPart(tempFrameVector); footForcesRaw.get(robotSide).set(tempFrameVector); tempWrench.getAngularPart(tempFrameVector); footTorquesRaw.get(robotSide).set(tempFrameVector); footForcesRawFiltered.get(robotSide).update(); footTorquesRawFiltered.get(robotSide).update(); } }
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(); }