private void updateKinematicsNewTwist() { rootJoint.getJointTwist(tempRootBodyTwist); rootJointLinearVelocityNewTwist.getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.changeFrame(tempRootBodyTwist.getExpressedInFrame()); tempRootBodyTwist.setLinearPart(tempFrameVector); rootJoint.setJointTwist(tempRootBodyTwist); twistCalculator.compute(); for (int i = 0; i < feetRigidBodies.size(); i++) { RigidBody foot = feetRigidBodies.get(i); Twist footTwistInWorld = footTwistsInWorld.get(foot); YoFrameVector footVelocityInWorld = footVelocitiesInWorld.get(foot); twistCalculator.getTwistOfBody(footTwistInWorld, foot); footTwistInWorld.changeBodyFrameNoRelativeTwist(soleFrames.get(foot)); footTwistInWorld.changeFrame(soleFrames.get(foot)); this.copsFilteredInFootFrame.get(foot).getFrameTuple2dIncludingFrame(tempCoP2d); tempCoP.setXYIncludingFrame(tempCoP2d); footTwistInWorld.changeFrame(footTwistInWorld.getBaseFrame()); tempCoP.changeFrame(footTwistInWorld.getExpressedInFrame()); footTwistInWorld.getLinearVelocityOfPointFixedInBodyFrame(tempFrameVector, tempCoP); tempFrameVector.changeFrame(worldFrame); footVelocityInWorld.set(tempFrameVector); } }
public void startComputation() { twistCalculator.getTwistOfBody(twist, rigidBody); twist.changeFrame(twist.getBaseFrame()); tempPointToMeasureVelocityOf.setIncludingFrame(pointToMeasureVelocityOf); tempPointToMeasureVelocityOf.changeFrame(twist.getBaseFrame()); twist.getLinearVelocityOfPointFixedInBodyFrame(pointVelocityFrameVector, tempPointToMeasureVelocityOf); pointVelocityFrameVector.changeFrame(worldFrame); pointVelocityFrameVector.get(pointVelocity); yoFrameVectorPerfect.set(pointVelocity); corrupt(pointVelocity); yoFrameVectorNoisy.set(pointVelocity); pointVelocityOutputPort.setData(pointVelocity); }
private void computeVelocityOfStationaryPoint(FrameVector stationaryPointVelocityToPack) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody stationaryPointLink = estimatorRigidBodyToIndexMap.getRigidBodyByName(pointVelocityMeasurementInputPort.getData().getRigidBodyName()); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); try { twistCalculator.getTwistOfBody(tempTwist, stationaryPointLink); } catch(Exception e) { e.printStackTrace(); } tempTwist.changeFrame(tempTwist.getBaseFrame()); ReferenceFrame referenceFrame = referenceFrameNameMap.getFrameByName(pointVelocityMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointVelocityMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(tempTwist.getBaseFrame()); tempTwist.getLinearVelocityOfPointFixedInBodyFrame(stationaryPointVelocityToPack, tempFramePoint); }
private void computeDerivativeTerm(FrameVector desiredVelocity, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); endEffectorTwist.changeFrame(baseFrame); bodyFixedPoint.setToZero(frameAtBodyFixedPoint); bodyFixedPoint.changeFrame(baseFrame); endEffectorTwist.getLinearVelocityOfPointFixedInBodyFrame(currentVelocity, bodyFixedPoint); desiredVelocity.changeFrame(baseFrame); currentVelocity.changeFrame(baseFrame); derivativeTerm.setToZero(baseFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); velocityError.setAndMatchFrame(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }