public static InverseDynamicsJoint[] computeSupportJoints(RigidBody... bodies) { Set<InverseDynamicsJoint> supportSet = new LinkedHashSet<InverseDynamicsJoint>(); for (RigidBody rigidBody : bodies) { RigidBody rootBody = getRootBody(rigidBody); InverseDynamicsJoint[] jointPath = createJointPath(rootBody, rigidBody); supportSet.addAll(Arrays.asList(jointPath)); } return supportSet.toArray(new InverseDynamicsJoint[supportSet.size()]); }
/** * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies * of the system to which {@code body} belongs. * * @param inertialFrame non-moving frame with respect to which the twists are computed. * Typically {@link ReferenceFrame#getWorldFrame()} is used here. * @param body a body that belongs to the system this twist calculator will be available for. */ public TwistCalculator(ReferenceFrame inertialFrame, RigidBody body) { this.rootBody = ScrewTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = ScrewTools.computeSubtreeSuccessors(ScrewTools.computeSubtreeJoints(rootBody)).length; while (unnassignedTwists.size() < numberOfRigidBodies) unnassignedTwists.add(new Twist()); assignedTwists = new ArrayList<>(numberOfRigidBodies); rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies); assignedTwists.add(rootTwist); rigidBodiesWithAssignedTwist.add(rootBody); rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0)); }
public JointTorqueFromForceSensorVisualizer(Map<RigidBody, FootSwitchInterface> footSwitches, YoVariableRegistry parentRegistry) { this.footSwitches = footSwitches; allRigidBodies = new ArrayList<>(footSwitches.keySet()); jacobians = new HashMap<>(); jointTorques = new HashMap<>(); for (RigidBody rigidBody : allRigidBodies) { RigidBody rootBody = ScrewTools.getRootBody(rigidBody); OneDoFJoint[] oneDoFJoints = ScrewTools.createOneDoFJointPath(rootBody, rigidBody); GeometricJacobian jacobian = new GeometricJacobian(oneDoFJoints, rigidBody.getBodyFixedFrame()); jacobians.put(rigidBody, jacobian); for (OneDoFJoint joint : oneDoFJoints) { if (!jointTorques.containsKey(joint)) { String variableName = "tau_forceSensor_" + joint.getName(); jointTorques.put(joint, new DoubleYoVariable(variableName, registry)); } } } parentRegistry.addChild(registry); }
totalRobotMass = TotalMassCalculator.computeSubTreeMass(fullRobotModel.getElevator()); else totalRobotMass = TotalMassCalculator.computeSubTreeMass(ScrewTools.getRootBody(controlledJoints[0].getSuccessor()));