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 void computeRigidBodiesAfterThisJoint(ArrayList<RigidBody> rigidBodySuccessorsToPack, InverseDynamicsJoint... joints) { ArrayList<InverseDynamicsJoint> jointStack = new ArrayList<InverseDynamicsJoint>(); for (InverseDynamicsJoint joint : joints) { jointStack.add(joint); } while (!jointStack.isEmpty()) { InverseDynamicsJoint currentJoint = jointStack.remove(0); rigidBodySuccessorsToPack.add(currentJoint.getSuccessor()); RigidBody currentBody = currentJoint.getSuccessor(); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { jointStack.add(joint); } } }
public static RigidBody[] computeSubtreeSuccessors(Set<InverseDynamicsJoint> jointsToExclude, RigidBody... bodies) { ArrayList<RigidBody> rigidBodySuccessors = new ArrayList<RigidBody>(); ArrayList<RigidBody> rigidBodyStack = new ArrayList<RigidBody>(); for (RigidBody body : bodies) { rigidBodyStack.add(body); } while (!rigidBodyStack.isEmpty()) { RigidBody currentBody = rigidBodyStack.remove(0); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { if (!jointsToExclude.contains(joint)) { rigidBodyStack.add(joint.getSuccessor()); rigidBodySuccessors.add(joint.getSuccessor()); } } } RigidBody[] ret = new RigidBody[rigidBodySuccessors.size()]; return rigidBodySuccessors.toArray(ret); }
public static RigidBody[] computeSubtreeSuccessors(InverseDynamicsJoint... joints) { ArrayList<RigidBody> rigidBodySuccessors = new ArrayList<RigidBody>(); ArrayList<RigidBody> rigidBodyStack = new ArrayList<RigidBody>(); for (InverseDynamicsJoint joint : joints) { rigidBodyStack.add(joint.getPredecessor()); } while (!rigidBodyStack.isEmpty()) { RigidBody currentBody = rigidBodyStack.remove(0); List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints) { rigidBodyStack.add(joint.getSuccessor()); rigidBodySuccessors.add(joint.getSuccessor()); } } RigidBody[] ret = new RigidBody[rigidBodySuccessors.size()]; return rigidBodySuccessors.toArray(ret); }
private void addAllSuccedingJoints(RigidBody body) { for (InverseDynamicsJoint inverseDynamicsJoint : body.getChildrenJoints()) { if (inverseDynamicsJoint.getSuccessor() != null) { allJoints.add(inverseDynamicsJoint); addAllSuccedingJoints(inverseDynamicsJoint.getSuccessor()); } } }
List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints)
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; } }
List<InverseDynamicsJoint> childrenJoints = currentBody.getChildrenJoints(); for (InverseDynamicsJoint joint : childrenJoints)
private void initialize() { hasBeenInitialized = true; TwistCalculator twistCalculator = new TwistCalculator(ReferenceFrame.getWorldFrame(), toolBody); boolean doVelocityTerms = true; boolean useDesireds = false; SpatialAccelerationCalculator spatialAccelerationCalculator = new SpatialAccelerationCalculator(toolBody, elevatorFrame, ScrewTools.createGravitationalSpatialAcceleration(fullRobotModel.getElevator(), gravity), twistCalculator, doVelocityTerms, useDesireds); ArrayList<InverseDynamicsJoint> jointsToIgnore = new ArrayList<InverseDynamicsJoint>(); jointsToIgnore.addAll(twistCalculator.getRootBody().getChildrenJoints()); jointsToIgnore.remove(toolJoint); inverseDynamicsCalculator = new InverseDynamicsCalculator(ReferenceFrame.getWorldFrame(), new LinkedHashMap<RigidBody, Wrench>(), jointsToIgnore, spatialAccelerationCalculator, twistCalculator, doVelocityTerms); }
private double getSubTreeMass(RigidBody rigidBody) { if (!subTreeMassMap.containsKey(rigidBody)) subTreeMassMap.put(rigidBody, new MutableDouble(-1.0)); MutableDouble subTreeMass = subTreeMassMap.get(rigidBody); if (subTreeMass.doubleValue() > 0.0) { return subTreeMass.doubleValue(); } else { double curSubTreeMass = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { double childSubTreeMass = getSubTreeMass(childrenJoints.get(i).getSuccessor()); curSubTreeMass = curSubTreeMass + childSubTreeMass; } subTreeMass.setValue(curSubTreeMass); return curSubTreeMass; } }
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 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; }
curChildCoMScaledByMass.scale(massToScale); final List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++)
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 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); } } }
this.totalWork[currentSample] = totalWorkVariable.getDoubleValue(); inverse.setAndInvert(rootBody.getChildrenJoints().get(0).getFrameAfterJoint().getTransformToRoot()); inverse.getTranslation(position); this.xPosition[currentSample] = position.getX();
List<InverseDynamicsJoint> childJoints = currentRigidBody.getChildrenJoints();