/** {@inheritDoc} */ @Override default int getJointVelocity(int rowStart, DenseMatrix64F matrixToPack) { getJointTwist().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointAcceleration(int rowStart, DenseMatrix64F matrixToPack) { getJointAcceleration().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** * Sets this joint configuration from the other joint. * * @param other the other to get the configuration from. Not modified. */ default void setJointConfiguration(SixDoFJointReadOnly other) { setJointConfiguration(other.getJointPose()); }
/** {@inheritDoc} */ @Override default int getJointTau(int rowStart, DenseMatrix64F matrixToPack) { getJointWrench().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointConfiguration(int rowStart, DenseMatrix64F matrixToPack) { getJointPose().getOrientation().get(rowStart, matrixToPack); getJointPose().getPosition().get(rowStart + 4, matrixToPack); return rowStart + getConfigurationMatrixSize(); }
private static SixDoFJointBasics cloneSixDoFJoint(SixDoFJointReadOnly original, String cloneSuffix, RigidBodyBasics clonePredecessor, JointBuilder jointBuilder) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = getCloneJointTransformToParent(original); return jointBuilder.buildSixDoFJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform); }
/** * Sets this joint velocity from the other joint. * * @param other the other to get the velocity from. Not modified. */ default void setJointTwist(SixDoFJointReadOnly 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 acceleration from the other joint. * * @param other the other to get the acceleration from. Not modified. */ default void setJointAcceleration(SixDoFJointReadOnly 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 force/torque from the other joint. * * @param other the other to get the force/torque from. Not modified. */ default void setJointWrench(SixDoFJointReadOnly 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); }