public void compute() { dynamicallyConsistentNullspaceCalculator.compute(); centerOfMassJacobian.compute(); DenseMatrix64F centerOfMassJacobianMatrix = centerOfMassJacobian.getMatrix(); DenseMatrix64F sNsBar = dynamicallyConsistentNullspaceCalculator.getSNsBar(); constrainedCenterOfMassJacobian.reshape(centerOfMassJacobianMatrix.getNumRows(), sNsBar.getNumCols()); CommonOps.mult(centerOfMassJacobianMatrix, sNsBar, constrainedCenterOfMassJacobian); }
public void update() { referenceFrames.updateFrames(); twistCalculator.compute(); centerOfMassJacobian.compute(); if (referenceFramesVisualizer != null) referenceFramesVisualizer.update(); computeCop(); computeCapturePoint(); updateBipedSupportPolygons(); readWristSensorData(); computeAngularMomentum(); robotJacobianHolder.compute(); for (int i = 0; i < updatables.size(); i++) updatables.get(i).update(yoTime.getDoubleValue()); for (RobotSide robotSide : RobotSide.values) footSwitches.get(robotSide).updateCoP(); }
private void updateGroundTruth() { FramePoint com = new FramePoint(); perfectCenterOfMassCalculator.compute(); perfectCenterOfMassCalculator.getCenterOfMass(com); perfectCoM.set(com); FrameVector comd = new FrameVector(); perfectCenterOfMassJacobian.compute(); perfectCenterOfMassJacobian.getCenterOfMassVelocity(comd); comd.changeFrame(ReferenceFrame.getWorldFrame()); perfectCoMd.set(comd); FrameVector comdd = new FrameVector(); perfectCenterOfMassAccelerationCalculator.getCoMAcceleration(comdd); comdd.changeFrame(ReferenceFrame.getWorldFrame()); perfectCoMdd.set(comdd); }
private void computeRootJointLinearVelocity(FrameVector centerOfMassVelocityWorld, FrameVector rootJointVelocityToPack, FrameVector rootJointAngularVelocity, FloatingInverseDynamicsJoint rootJoint) { tempCenterOfMassVelocityWorld.setIncludingFrame(centerOfMassVelocityWorld); ReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); // \dot{r}^{root} centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame); // \tilde{\omega} r^{root} tempComBody.setToZero(rootJointFrame); centerOfMassCalculator.getCenterOfMass(tempComBody); tempComBody.changeFrame(rootJointFrame); tempCrossPart.setToZero(rootJointFrame); tempCrossPart.cross(rootJointAngularVelocity, tempComBody); // v_{r/p}= \tilde{\omega} r^{root} + \dot{r}^{root} tempCenterOfMassVelocityOffset.setToZero(rootJointFrame); tempCenterOfMassVelocityOffset.add(tempCrossPart, tempComVelocityBody); // v_{root}^{p,w} = R_{w}^{root} \dot{r} - v_{r/p} tempCenterOfMassVelocityWorld.changeFrame(rootJointFrame); rootJointVelocityToPack.setIncludingFrame(tempCenterOfMassVelocityWorld); rootJointVelocityToPack.sub(tempCenterOfMassVelocityOffset); }
yoCenterOfMassPosition.set(centerOfMassPosition); centerOfMassJacobianWorld.compute(); centerOfMassVelocityUsingPelvisIMUAndKinematics.setToZero(ReferenceFrame.getWorldFrame()); centerOfMassJacobianWorld.getCenterOfMassVelocity(centerOfMassVelocityUsingPelvisIMUAndKinematics);
centerOfMassJacobianBody.compute(); tempComVelocityBody.setToZero(rootJointFrame); centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody);