Fix reference frame mismatch error in new planner

aprvs · aprvs · commit 4469a6bd65d2 · 2017-08-16T10:47:01.000-05:00
diff --git a/CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/instantaneousCapturePoint/smoothCMP/ReferenceCoPTrajectoryGenerator.java b/CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/instantaneousCapturePoint/smoothCMP/ReferenceCoPTrajectoryGenerator.java
@@ -586,6 +586,7 @@ private void computeMidFeetPointByPositionFraction(FramePoint3D framePointToPack
       }
       getDoubleSupportPolygonCentroid(tempDoubleSupportPolygonCentroid, footPolygonA, footPolygonB, referenceFrameToConvertTo);
       convertToFramePointRetainingZ(tempFramePoint1, framePolygonReference.getCentroid(), referenceFrameToConvertTo);
+      framePointToPack.changeFrame(referenceFrameToConvertTo);
       framePointToPack.interpolate(this.tempDoubleSupportPolygonCentroid, tempFramePoint1, this.tempDouble);
    }
 
@@ -977,6 +978,7 @@ private void getDoubleSupportPolygonCentroid(FramePoint3D framePointToPack, Fram
       tempFramePoint1.changeFrame(referenceFrameToStoreResultIn);
       tempFramePoint2.setIncludingFrame(supportFootPolygon.getCentroid(), 0.0);
       tempFramePoint2.changeFrame(referenceFrameToStoreResultIn);
+      framePointToPack.changeFrame(referenceFrameToStoreResultIn);
       framePointToPack.interpolate(tempFramePoint1, tempFramePoint2, 0.5);
    }
 

Original file line number	Diff line number	Diff line change
`@@ -586,6 +586,7 @@ private void computeMidFeetPointByPositionFraction(FramePoint3D framePointToPack`
`586`	`586`	`}`
`587`	`587`	`getDoubleSupportPolygonCentroid(tempDoubleSupportPolygonCentroid, footPolygonA, footPolygonB, referenceFrameToConvertTo);`
`588`	`588`	`convertToFramePointRetainingZ(tempFramePoint1, framePolygonReference.getCentroid(), referenceFrameToConvertTo);`
	`589`	`+ framePointToPack.changeFrame(referenceFrameToConvertTo);`
`589`	`590`	`framePointToPack.interpolate(this.tempDoubleSupportPolygonCentroid, tempFramePoint1, this.tempDouble);`
`590`	`591`	`}`
`591`	`592`
`@@ -977,6 +978,7 @@ private void getDoubleSupportPolygonCentroid(FramePoint3D framePointToPack, Fram`
`977`	`978`	`tempFramePoint1.changeFrame(referenceFrameToStoreResultIn);`
`978`	`979`	`tempFramePoint2.setIncludingFrame(supportFootPolygon.getCentroid(), 0.0);`
`979`	`980`	`tempFramePoint2.changeFrame(referenceFrameToStoreResultIn);`
	`981`	`+ framePointToPack.changeFrame(referenceFrameToStoreResultIn);`
`980`	`982`	`framePointToPack.interpolate(tempFramePoint1, tempFramePoint2, 0.5);`
`981`	`983`	`}`
`982`	`984`