private Point3D computeTouchdownCoM() { int touchdownCount = 0; Point3D touchdownCoM = new Point3D(); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownCoM.add(touchdownLocation); touchdownCount++; } } touchdownCoM.scale(1.0/touchdownCount); return touchdownCoM; }
private void applyTranslationalSlip(double percentOfDelta) { FrameVector3D slipDelta = new FrameVector3D(slipAmount); slipDelta.scale(percentOfDelta); slipAmount.sub(slipDelta); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownLocation.add(slipDelta); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
private void applyRotationalSlip(double percentOfDelta) { FrameQuaternion identity = new FrameQuaternion(ReferenceFrame.getWorldFrame()); FrameQuaternion desired = slipRotation.getFrameOrientationCopy(); FrameQuaternion delta = new FrameQuaternion(); delta.interpolate(identity, desired, percentOfDelta); desired.interpolate(identity, desired, 1.0-percentOfDelta); slipRotation.set(desired); Point3D touchdownCoM = computeTouchdownCoM(); RotationMatrix deltaRotation = new RotationMatrix(delta); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownLocation.sub(touchdownCoM); deltaRotation.transform(touchdownLocation); touchdownLocation.add(touchdownCoM); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
groundContactPoint.getTouchdownLocation(touchdownPosition); groundContactPoint.getTouchdownLocation(touchdownTest);