@Override public ReferenceFrame getHeadBaseFrame() { if(head != null) { JointBasics headJoint = head.getParentJoint(); return headJoint.getFrameAfterJoint(); } return null; }
/** * 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()); }
public String getParentJointName() { return contactingRigidBody.getParentJoint().getName(); }
public void setJointLimits(OneDoFJointBasics joint) { positionSoftUpperLimit = joint.getJointLimitUpper(); positionSoftLowerLimit = joint.getJointLimitLower(); velocityLimitUpper = joint.getVelocityLimitUpper(); velocityLimitLower = joint.getVelocityLimitLower(); torqueLimitUpper = joint.getEffortLimitUpper(); torqueLimitLower = joint.getEffortLimitLower(); }
private void setBestJointAngles() { for (int i = 0; i < oneDoFJoints.length; i++) { oneDoFJoints[i].setQ(bestJointAngles[i]); } }
@Override protected void copyJoint(JointBasics originalJoint, JointBasics targetJoint) { targetJoint.setJointConfiguration(originalJoint); targetJoint.setJointTwist(originalJoint); targetJoint.setJointAcceleration(originalJoint); }
@Override public double getJointTauProcessedOutput(OneDoFJointBasics oneDoFJoint) { return oneDoFJoint.getTau(); }
/** * Gets the {@code JointMatrixIndexProvider} to use with this input. * * @return the matrix index provider for the considered joints. */ default JointMatrixIndexProvider getJointMatrixIndexProvider() { return JointMatrixIndexProvider.toIndexProvider(getJointsToConsider()); }
/** * Creates a new calculator for the given {@code input}. * * @param input the definition of the system to be evaluated by this calculator. */ public CompositeRigidBodyMassMatrixCalculator(MultiBodySystemReadOnly input) { this(input, input.getRootBody().getBodyFixedFrame()); }
/** * 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()); }
/** * Sets this joint acceleration from the other joint. * * @param other the other to get the acceleration from. Not modified. */ default void setJointAcceleration(SphericalJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherAngularAcceleration = other.getJointAngularAcceleration(); setJointAngularAcceleration(otherAngularAcceleration); }
/** * Sets this joint force/torque from the other joint. * * @param other the other to get the force/torque from. Not modified. */ default void setJointWrench(SphericalJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherTorque = other.getJointTorque(); setJointTorque(otherTorque); }
/** * Sets this joint velocity from the other joint. * * @param other the other to get the velocity from. Not modified. */ default void setJointTwist(SphericalJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherAngularVelocity = other.getJointAngularVelocity(); setJointAngularVelocity(otherAngularVelocity); }
/** * Returns the body fixed frame of the base {@code RigidBody} of the current Jacobian. The base is * the predecessor of the first joint that the Jacobian considers. * * @return the body fixed frame of the base. */ public ReferenceFrame getBaseFrame() { return base.getBodyFixedFrame(); }
/** * Creates a new calculator for the subtree that starts off the given {@code rootBody}. * * @param rootBody the start of subtree for which the centroidal momentum matrix is to be computed. * Not modified. * @param matrixFrame the frame in which the centroidal momentum matrix is to be expressed. */ public CentroidalMomentumCalculator(RigidBodyReadOnly rootBody, ReferenceFrame matrixFrame) { this(MultiBodySystemReadOnly.toMultiBodySystemInput(rootBody), matrixFrame); }
@Override public ReferenceFrame getHeadBaseFrame() { return head.getParentJoint().getFrameAfterJoint(); }