public void reset() { for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) { Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getExpressedInFrame()); } }
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); }
protected void getYoValuesFromWrench() { linearPart.set(wrench.getExpressedInFrame(), wrench.getLinearPartX(), wrench.getLinearPartY(), wrench.getLinearPartZ()); angularPart.set(wrench.getExpressedInFrame(), wrench.getAngularPartX(), wrench.getAngularPartY(), wrench.getAngularPartZ()); }
public void setWrench(Wrench newWrench) { measurementFrame.checkReferenceFrameMatch(newWrench.getExpressedInFrame()); measurementFrame.checkReferenceFrameMatch(newWrench.getBodyFrame()); newWrench.getMatrix(wrench); }
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 setWrench(Wrench jointWrench) { successorWrench.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); successorWrench.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); successorWrench.setAngularPartY(jointWrench.getAngularPartY()); successorWrench.setLinearPartX(jointWrench.getLinearPartX()); successorWrench.setLinearPartZ(jointWrench.getLinearPartZ()); }
@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) { unitSuccessorTwist.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); unitSuccessorTwist.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); this.tau = unitSuccessorTwist.dot(jointWrench); // cheating a little bit; tau = J^T * wrench. J maps joint velocities to joint twists. // the unit twist is actually exactly the same as J, except that its entries have different dimensions. // we disregard dimensions and just use .dot(.) for efficiency }
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); }
tempVector.setToZero(tempWrench.getExpressedInFrame()); tempWrench.getLinearPart(tempVector); tempVector.changeFrame(ReferenceFrame.getWorldFrame()); tempVector.setToZero(tempWrench.getExpressedInFrame()); tempWrench.getAngularPart(tempVector); tempVector.changeFrame(ReferenceFrame.getWorldFrame());
/** * Computes and packs the joint torque vector that corresponds to the given wrench. * * @param wrench the resulting wrench at the end effector. * The wrench should be expressed in {@code jacobianFrame} and the wrench's {@code bodyFrame} * should be the body fixed frame of the end-effector. * @throws ReferenceFrameMismatchException if the given wrench {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. */ public void computeJointTorques(Wrench wrench, DenseMatrix64F jointTorquesToPack) { // reference frame check wrench.getExpressedInFrame().checkReferenceFrameMatch(this.jacobianFrame); // FIXME add the following reference frame check // wrench.getBodyFrame().checkReferenceFrameMatch(getEndEffectorFrame()); wrench.getMatrix(tempMatrix); jointTorquesToPack.reshape(1, jacobian.getNumCols()); CommonOps.multTransA(tempMatrix, jacobian, jointTorquesToPack); CommonOps.transpose(jointTorquesToPack); }
@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; }
public void getWrench(Wrench wrenchToPack) { wrenchToPack.setToZero(successorWrench.getBodyFrame(), successorWrench.getExpressedInFrame()); wrenchToPack.setAngularPartY(successorWrench.getAngularPartY()); wrenchToPack.setLinearPartX(successorWrench.getLinearPartX()); wrenchToPack.setLinearPartZ(successorWrench.getLinearPartZ()); }
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(); }
@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()); }
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] }