/** * Computes the angular velocity for an interpolation between two orientations using the SLERP method. * @param startOrientation the starting orientation * @param endOrientation the final orientation * @param alphaDot the interpolation rate * @return the angular velocity of the interpolated frame, w.r.t. the startOrientation, expressed in the frame in which the orientations were expressed */ public void computeAngularVelocity(YoFrameVector angularVelocityToPack, YoFrameQuaternion startOrientation, YoFrameQuaternion endOrientation, double alphaDot) { angularVelocityToPack.checkReferenceFrameMatch(startOrientation); startOrientation.checkReferenceFrameMatch(endOrientation); startOrientation.get(startRotationQuaternion); endOrientation.get(endRotationQuaternion); computeAngularVelocity(angularVelocity, startRotationQuaternion, endRotationQuaternion, alphaDot); angularVelocityToPack.set(angularVelocity); }
public void setDesiredCapturePointState(YoFramePoint2d currentDesiredCapturePointPosition, YoFrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
public void setDesiredCapturePointState(FramePoint2d currentDesiredCapturePointPosition, FrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }