public void getCenterOfMassVelocity(FrameVector centerOfMassVelocityToPack) { ScrewTools.getJointVelocitiesMatrix(joints, tempJointVelocitiesMatrix); getCenterOfMassVelocity(centerOfMassVelocityToPack, tempJointVelocitiesMatrix); }
private void computeCapturePoint() { centerOfMassPosition.setToZero(centerOfMassFrame); if (centerOfMassDataHolder != null) { centerOfMassDataHolder.getCenterOfMassVelocity(centerOfMassVelocity); } else { centerOfMassJacobian.getCenterOfMassVelocity(centerOfMassVelocity); } centerOfMassPosition.changeFrame(worldFrame); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); centerOfMassVelocity2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassVelocity); CapturePointCalculator.computeCapturePoint(capturePoint2d, centerOfMassPosition2d, centerOfMassVelocity2d, omega0.getDoubleValue()); capturePoint2d.changeFrame(yoCapturePoint.getReferenceFrame()); yoCapturePoint.setXY(capturePoint2d); }
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); }
centerOfMassJacobianWorld.getCenterOfMassVelocity(centerOfMassVelocityUsingPelvisIMUAndKinematics); centerOfMassVelocityUsingPelvisIMUAndKinematics.changeFrame(ReferenceFrame.getWorldFrame()); yoCenterOfMassVelocityUsingPelvisAndKinematics.set(centerOfMassVelocityUsingPelvisIMUAndKinematics);
centerOfMassJacobian.getCenterOfMassVelocity(comVelocity); comPosition.changeFrame(worldFrame); comVelocity.changeFrame(worldFrame);
centerOfMassJacobianBody.getCenterOfMassVelocity(tempComVelocityBody); tempComVelocityBody.changeFrame(rootJointFrame);