/** * Returns the body fixed frame of the base {@code RigidBody} of this Jacobian. * The base is the predecessor of the first joint that this Jacobian considers. * * @return the body fixed frame of the base. */ public ReferenceFrame getBaseFrame() { return getBase().getBodyFixedFrame(); }
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis, boolean isPartOfClosedKinematicLoop) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; return new PassiveRevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis), isPartOfClosedKinematicLoop); }
@Override public String toString() { return "ForceSensorDefinition: " + sensorName + " attached to " + rigidBody.getName(); }
public YoPointVelocityDataObject(String namePrefix, RigidBody body, YoVariableRegistry registry) { this(namePrefix, body.getParentJoint().getFrameAfterJoint(), registry); rigidBodyName = body.getName(); }
public static boolean isAncestor(RigidBody candidateDescendant, RigidBody ancestor) { RigidBody currentBody = candidateDescendant; while (!currentBody.isRootBody()) { if (currentBody == ancestor) { return true; } currentBody = currentBody.getParentJoint().getPredecessor(); } return currentBody == ancestor; }
@Override public void variableChanged(YoVariable<?> v) { rigidBodyCoMOffset.getFrameTupleIncludingFrame(tempFramePoint); tempFramePoint.changeFrame(rigidBody.getBodyFixedFrame()); rigidBody.setCoMOffset(tempFramePoint); } };
public static RigidBody getRootBody(RigidBody body) { RigidBody ret = body; while (ret.getParentJoint() != null) { ret = ret.getParentJoint().getPredecessor(); } return ret; }
private void printLinkInformation(RigidBody link, StringBuffer buffer) { RigidBodyInertia inertia = link.getInertia(); InverseDynamicsJoint parentJoint = link.getParentJoint(); if (inertia != null) { double mass = inertia.getMass(); Vector3d comOffset = new Vector3d(); RigidBodyTransform comOffsetTransform = link.getBodyFixedFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); comOffsetTransform.getTranslation(comOffset); Matrix3d momentOfInertia = inertia.getMassMomentOfInertiaPartCopy(); buffer.append("Mass = " + mass + "\n"); buffer.append("comOffset = " + comOffset + "\n"); buffer.append("momentOfInertia = \n" + momentOfInertia + "\n"); } List<InverseDynamicsJoint> childrenJoints = link.getChildrenJoints(); for (InverseDynamicsJoint childJoint : childrenJoints) { String parentJointName; if (parentJoint != null) parentJointName = parentJoint.getName(); else parentJointName = "root joint"; buffer.append("Found Child Joint of " + parentJointName + ".\n"); printJointInformation(childJoint, buffer); } }
private static RigidBody cloneRigidBody(RigidBody original, String cloneSuffix, InverseDynamicsJoint parentJointOfClone) { FramePoint comOffset = new FramePoint(); original.getCoMOffset(comOffset); comOffset.changeFrame(original.getParentJoint().getFrameAfterJoint()); String nameOriginal = original.getName(); Matrix3d massMomentOfInertiaPartCopy = original.getInertia().getMassMomentOfInertiaPartCopy(); double mass = original.getInertia().getMass(); Vector3d comOffsetCopy = comOffset.getVectorCopy(); RigidBody clone = ScrewTools.addRigidBody(nameOriginal + cloneSuffix, parentJointOfClone, massMomentOfInertiaPartCopy, mass, comOffsetCopy); return clone; }
public void updateFramesRecursively() { elevator.updateFramesRecursively(); }
SideDependentList<SixDoFJoint> sixDoFJoints = new SideDependentList<SixDoFJoint>(); RigidBody rootBody = new RigidBody("Root", ReferenceFrame.getWorldFrame()); Random random = new Random(1776L); soleFrames.set(robotSide, soleFrame); SixDoFJoint sixDoFJoint = new SixDoFJoint("SixDofJoint" + robotSide, rootBody, rootBody.getBodyFixedFrame()); sixDoFJoints.set(robotSide, sixDoFJoint);
public MassMatrixCalculatorComparer() { joints = new ArrayList<RevoluteJoint>(); elevator = new RigidBody("elevator", worldFrame); Vector3d[] jointAxes = {X, Y, Z, Z, X, Z, Z, X, Y, Y}; ScrewTestTools.createRandomChainRobot("", joints, elevator, jointAxes, random); massMatrixCalculators.add(new DifferentialIDMassMatrixCalculator(worldFrame, elevator)); massMatrixCalculators.add(new CompositeRigidBodyMassMatrixCalculator(elevator)); }
private void computeJointWrenchesAndTorques() { for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) { InverseDynamicsJoint joint = allJoints.get(jointIndex); RigidBody successor = joint.getSuccessor(); Wrench jointWrench = jointWrenches.get(joint); jointWrench.set(netWrenches.get(successor)); Wrench externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); List<InverseDynamicsJoint> childrenJoints = successor.getChildrenJoints(); for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) { InverseDynamicsJoint child = childrenJoints.get(childIndex); if (!jointsToIgnore.contains(child)) { Wrench wrenchExertedOnChild = jointWrenches.get(child); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); wrenchExertedByChild.set(wrenchExertedOnChild); wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame); wrenchExertedByChild.scale(-1.0); // Action = -reaction wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame()); jointWrench.sub(wrenchExertedByChild); } } joint.setTorqueFromWrench(jointWrench); } }
public static InverseDynamicsJoint[] cloneJointPathDisconnectedFromOriginalRobot(InverseDynamicsJoint[] inverseDynamicsJoints, String suffix, ReferenceFrame rootBodyFrame) { InverseDynamicsJoint[] cloned = new InverseDynamicsJoint[inverseDynamicsJoints.length]; for (int i = 0; i < inverseDynamicsJoints.length; i++) { if (inverseDynamicsJoints[i] instanceof RevoluteJoint) { RevoluteJoint jointOriginal = (RevoluteJoint) inverseDynamicsJoints[i]; RigidBody predecessorOriginal = jointOriginal.getPredecessor(); RigidBody predecessorCopy; if (i > 0) { predecessorCopy = cloned[i - 1].getSuccessor(); } else { String predecessorNameOriginal = predecessorOriginal.getName(); predecessorCopy = new RigidBody(predecessorNameOriginal + suffix, rootBodyFrame); } cloned[i] = cloneOneDoFJoint(jointOriginal, suffix, predecessorCopy); } else { throw new RuntimeException("Not implemented for joints of the type: " + inverseDynamicsJoints[i].getClass().getSimpleName()); } cloneRigidBody(inverseDynamicsJoints[i].getSuccessor(), suffix, cloned[i]); } return cloned; }
private void findMinimumAndMaximumMassOfRigidBodies(RigidBody body) { RigidBodyInertia inertia = body.getInertia(); if (inertia != null) { double mass = body.getInertia().getMass(); if (mass < minimumMassOfRigidBodies.getDoubleValue() && mass > 1e-3) minimumMassOfRigidBodies.set(mass); if (mass > maximumMassOfRigidBodies.getDoubleValue()) maximumMassOfRigidBodies.set(mass); } if (body.hasChildrenJoints()) { List<InverseDynamicsJoint> childJoints = body.getChildrenJoints(); for (InverseDynamicsJoint joint : childJoints) { RigidBody nextBody = joint.getSuccessor(); if (nextBody != null) findMinimumAndMaximumMassOfRigidBodies(nextBody); } } }
rigidBody.getCoMOffset(curChildCoMScaledByMass); curChildCoMScaledByMass.changeFrame(rootFrame); double massToScale = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); curChildCoMScaledByMass.scale(massToScale); final List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++)
public static InverseDynamicsJoint[] computeSubtreeJoints(List<RigidBody> rootBodies) { ArrayList<InverseDynamicsJoint> subtree = new ArrayList<InverseDynamicsJoint>(); ArrayList<RigidBody> rigidBodyStack = new ArrayList<RigidBody>(); rigidBodyStack.addAll(rootBodies); while (!rigidBodyStack.isEmpty()) { RigidBody currentBody = rigidBodyStack.remove(0); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { RigidBody successor = joint.getSuccessor(); rigidBodyStack.add(successor); subtree.add(joint); } } InverseDynamicsJoint[] ret = new InverseDynamicsJoint[subtree.size()]; return subtree.toArray(ret); }
public static double computeSubTreeMass(RigidBody rootBody) { RigidBodyInertia inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (InverseDynamicsJoint childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
public static PassiveRevoluteJoint getFourBarOutputJoint(PassiveRevoluteJoint passiveJointB, PassiveRevoluteJoint passiveJointC, PassiveRevoluteJoint passiveJointD) { // If the output joint is D then it will have at least 1 child, otherwise it won't have any if(passiveJointD.getSuccessor().hasChildrenJoints()) { return passiveJointD; } // Joint C wil only have joint D as its child, unless it's the output joint of the fourbar else if (passiveJointC.getSuccessor().getChildrenJoints().size() > 1) { return passiveJointC; } else { return passiveJointB; } }
private static int[] createMassMatrixIndices(RigidBody[] rigidBodiesInOrder) { int[] ret = new int[rigidBodiesInOrder.length]; int currentIndex = 0; for (int i = 0; i < rigidBodiesInOrder.length; i++) { RigidBody rigidBody = rigidBodiesInOrder[i]; ret[i] = currentIndex; currentIndex += rigidBody.getParentJoint().getDegreesOfFreedom(); } return ret; }