/** * Sets all the components of this vector to zero and updates its reference frames. * * @param bodyFrame the new body frame. * @param baseFrame the new base frame. * @param expressedInFrame the new reference frame in which this motion is expressed. */ default void setToZero(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setToZero(expressedInFrame); }
/** * Sets all the components of this vector to {@link Double#NaN} and sets its reference frames. * * @param bodyFrame the new body frame. * @param baseFrame the new base frame. * @param expressedInFrame the new reference frame in which this motion is expressed. */ default void setToNaN(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setToNaN(expressedInFrame); }
/** * Sets this motion vector given an angular part and linear part and updates all its reference * frames. * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param angularPart the vector holding the new values for the angular part. Not modified. * @param linearPart the vector holding the new values for the linear part. Not modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, Vector3DReadOnly angularPart, Vector3DReadOnly linearPart) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, angularPart, linearPart); }
/** * Sets this motion vector's components from the given column vector starting to read from * {@code startRow} and updates all its reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param matrix the column vector containing the new values for this motion vector's components. * Not modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, DenseMatrix64F matrix) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, matrix); }
/** * Sets this motion vector's components from the given column vector starting to read from * {@code startRow} at the column index {@code column} and updates all its reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param startRow the first row index to start reading in the dense-matrix. * @param matrix the column vector containing the new values for this motion vector's components. * Not modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, int startRow, DenseMatrix64F matrix) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, startRow, matrix); }
/** * Sets this motion vector's components from the given array {@code array} and updates all its * reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param array the array containing the new values for this motion vector's components. Not * modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, double[] array) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, array); }
/** * Sets this motion vector's components from the given array {@code array} and updates all its * reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param array the array containing the new values for this motion vector's components. Not * modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, float[] array) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, array); }
/** * Sets this motion vector's components from the given array {@code array} and updates all its * reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param startIndex the first index to start reading from in the array. * @param array the array containing the new values for this motion vector's components. Not * modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, int startIndex, float[] array) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, startIndex, array); }
/** * Sets this motion vector's components from the given column vector starting to read from its * first row index and updates all its reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param startRow the first row index to start reading in the dense-matrix. * @param column the column index to read in the dense-matrix. * @param matrix the column vector containing the new values for this motion vector's components. * Not modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, int startRow, int column, DenseMatrix64F matrix) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, startRow, column, matrix); }
/** * Sets this motion vector's components from the given array {@code array} and updates all its * reference frames. * <p> * The components are read in the following order: {@code angularPartX}, {@code angularPartY}, * {@code angularPartZ}, {@code linearPartX}, {@code linearPartY}, {@code linearPartZ}. * </p> * * @param bodyFrame what we are specifying the motion of. * @param baseFrame with respect to what we are specifying the motion. * @param expressedInFrame in which reference frame the motion is expressed. * @param startIndex the first index to start reading from in the array. * @param array the array containing the new values for this motion vector's components. Not * modified. */ default void setIncludingFrame(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, int startIndex, double[] array) { setBodyFrame(bodyFrame); setBaseFrame(baseFrame); setIncludingFrame(expressedInFrame, startIndex, array); }
/** * Inverts this spatial motion vector such that after calling this method, this motion vector * describes the motion of the base with respect to the body. * <p> * In other words: given the spatial motion of frame A with respect to frame B, expressed in * frame C, this method computes the spatial motion of frame B with respect to frame A, expressed * in frame C (or vice versa), by just taking the additive inverse. * </p> * <p> * See Duindam, <i>Port-Based Modeling and Control for Efficient Bipedal Walking Robots</i>, page * 25, lemma 2.8 (d) (and (e), for generalizing to any expressedInFrame). * </p> * <p> * While Duindam proves this fact for twists only, it can be easily proved to be accurate for * derivatives of twists. * </p> */ default void invert() { negate(); ReferenceFrame oldBaseFrame = getBaseFrame(); setBaseFrame(getBodyFrame()); setBodyFrame(oldBaseFrame); } }