private double[] getArcLengthsApproximatedByDistance(int[] indices) { double[] arcLengths = new double[indices.length - 1]; for (int i = 0; i < indices.length - 1; i++) { arcLengths[i] = allPositions[indices[i]].distance(allPositions[indices[i + 1]]); } return arcLengths; }
public double getDistance(YoFramePose goalYoPose) { return position.distance(goalYoPose.getPosition()); }
public double distance(YoFramePoint yoFramePoint) { return distance(yoFramePoint.getFrameTuple()); }
public double getDistance(YoFramePose goalYoPose) { return position.distance(goalYoPose.getPosition()); }
/** * Compute the desired capture point position at a given time. * x<sup>ICP<sub>des</sub></sup> = (e<sup>ω0 t</sup>) x<sup>ICP<sub>0</sub></sup> + (1-e<sup>ω0 t</sup>)x<sup>CMP<sub>0</sub></sup> * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointToPack */ public static void computeDesiredCapturePointPosition(double omega0, double time, YoFramePoint initialCapturePoint, YoFramePoint initialCMP, YoFramePoint desiredCapturePointToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); }
/** * Compute the desired capture point velocity at a given time. * ICPv_d = w^2 * e^{w*t} * ICP0 - p0 * w^2 * e^{w*t} * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointAccelerationToPack */ public static void computeDesiredCapturePointAcceleration(double omega0, double time, YoFramePoint initialCapturePoint, YoFramePoint initialCMP, YoFrameVector desiredCapturePointAccelerationToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointAccelerationToPack.subAndScale(omega0 * omega0 * Math.exp(omega0 * time), initialCapturePoint, initialCMP); else desiredCapturePointAccelerationToPack.setToZero(); }
/** * Compute the desired capture point velocity at a given time. * ICPv_d = w * e^{w*t} * ICP0 - p0 * w * e^{w*t} * * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointVelocityToPack */ public static void computeDesiredCapturePointVelocity(double omega0, double time, YoFramePoint initialCapturePoint, YoFramePoint initialCMP, YoFrameVector desiredCapturePointVelocityToPack) { if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointVelocityToPack.subAndScale(omega0 * Math.exp(omega0 * time), initialCapturePoint, initialCMP); else desiredCapturePointVelocityToPack.setToZero(); }