/** {@inheritDoc} */ @Override default int getConfigurationMatrixSize() { return getDegreesOfFreedom(); } }
/** * Sets this joint configuration from the other joint. * * @param other the other to get the configuration from. Not modified. */ default void setJointConfiguration(PlanarJointReadOnly other) { setJointConfiguration(other.getJointPose()); }
/** {@inheritDoc} */ @Override default int getJointConfiguration(int rowStart, DenseMatrix64F matrixToPack) { int index = rowStart; matrixToPack.set(index++, 0, getJointPose().getPitch()); matrixToPack.set(index++, 0, getJointPose().getX()); matrixToPack.set(index++, 0, getJointPose().getZ()); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointAcceleration(int rowStart, DenseMatrix64F matrixToPack) { matrixToPack.set(rowStart + 0, 0, getJointAcceleration().getAngularPartY()); matrixToPack.set(rowStart + 1, 0, getJointAcceleration().getLinearPartX()); matrixToPack.set(rowStart + 2, 0, getJointAcceleration().getLinearPartZ()); return rowStart + getConfigurationMatrixSize(); }
/** {@inheritDoc} */ @Override default int getJointTau(int rowStart, DenseMatrix64F matrixToPack) { matrixToPack.set(rowStart + 0, 0, getJointWrench().getAngularPartY()); matrixToPack.set(rowStart + 1, 0, getJointWrench().getLinearPartX()); matrixToPack.set(rowStart + 2, 0, getJointWrench().getLinearPartZ()); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointVelocity(int rowStart, DenseMatrix64F matrixToPack) { matrixToPack.set(rowStart + 0, 0, getJointTwist().getAngularPartY()); matrixToPack.set(rowStart + 1, 0, getJointTwist().getLinearPartX()); matrixToPack.set(rowStart + 2, 0, getJointTwist().getLinearPartZ()); return rowStart + getDegreesOfFreedom(); }
private static PlanarJoint clonePlanarJoint(PlanarJointReadOnly original, String cloneSuffix, RigidBodyBasics clonePredecessor, JointBuilder jointBuilder) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = getCloneJointTransformToParent(original); return new PlanarJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform); }
/** * Sets this joint acceleration from the other joint. * * @param other the other to get the acceleration from. Not modified. */ default void setJointAcceleration(PlanarJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherAngularAcceleration = other.getJointAcceleration().getAngularPart(); Vector3DReadOnly otherLinearAcceleration = other.getJointAcceleration().getLinearPart(); setJointAngularAcceleration(otherAngularAcceleration); setJointLinearAcceleration(otherLinearAcceleration); }
/** * Sets this joint velocity from the other joint. * * @param other the other to get the velocity from. Not modified. */ default void setJointTwist(PlanarJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherAngularVelocity = other.getJointTwist().getAngularPart(); Vector3DReadOnly otherLinearVelocity = other.getJointTwist().getLinearPart(); setJointAngularVelocity(otherAngularVelocity); setJointLinearVelocity(otherLinearVelocity); }
/** * Sets this joint force/torque from the other joint. * * @param other the other to get the force/torque from. Not modified. */ default void setJointWrench(PlanarJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherTorque = other.getJointWrench().getAngularPart(); Vector3DReadOnly otherForce = other.getJointWrench().getLinearPart(); setJointTorque(otherTorque); setJointForce(otherForce); }