public JointConfigurationDataReceiver(RigidBody rootBody) { // this.joints = ScrewTools.computeJointsInOrder(rootBody); //deprecated method this.joints = ScrewTools.computeSubtreeJoints(rootBody); } public synchronized void receivedPacket(JointConfigurationData jointConfigurationData)
public AbstractInverseDynamicsChecksum(RigidBody rootJoint, GenericCRC32 checksum) { this.checksum = checksum; this.joints = ScrewTools.computeSubtreeJoints(rootJoint); }
public JointConfigurationDataSender(RigidBody rootBody, PacketCommunicator packetCommunicator) { // this.joints = ScrewTools.computeJointsInOrder(rootBody); //deprecated method this.joints = ScrewTools.computeSubtreeJoints(rootBody); this.packetCommunicator = packetCommunicator; }
public static InverseDynamicsJoint[] computeSubtreeJoints(RigidBody... rootBodies) { return computeSubtreeJoints(Arrays.asList(rootBodies)); }
public void setRigidBodies(RigidBody originalBody, RigidBody targetBody) { jointPairs.clear(); InverseDynamicsJoint[] originalJoints = ScrewTools.computeSubtreeJoints(originalBody); InverseDynamicsJoint[] targetJoints = ScrewTools.computeSubtreeJoints(targetBody); for(int i = 0; i < originalJoints.length; i++) { InverseDynamicsJoint originalJoint = originalJoints[i]; InverseDynamicsJoint targetJoint = targetJoints[i]; areJointsTheSame(originalJoint, targetJoint); ImmutablePair<InverseDynamicsJoint, InverseDynamicsJoint> jointPair = new ImmutablePair<InverseDynamicsJoint, InverseDynamicsJoint>(originalJoint, targetJoint); jointPairs.add(jointPair); } }
public static InverseDynamicsJoint[] computeSupportAndSubtreeJoints(RigidBody... bodies) { Set<InverseDynamicsJoint> ret = new LinkedHashSet<InverseDynamicsJoint>(); for (RigidBody body : bodies) { ret.addAll(Arrays.asList(computeSupportJoints(body))); ret.addAll(Arrays.asList(computeSubtreeJoints(body))); } return ret.toArray(new InverseDynamicsJoint[ret.size()]); }
public static RigidBody[] computeSubtreeSuccessors(RigidBody... bodies) { return computeSuccessors(computeSubtreeJoints(bodies)); }
public static List<PrismaticJoint> createRandomTreeRobotWithPrismaticJoints(String prefix, RigidBody rootBody, int numberOfJoints, Random random) { List<PrismaticJoint> tempJoints = new ArrayList<>(); RigidBody predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { PrismaticJoint joint = addRandomPrismaticJoint(prefix + "Joint" + i, random, predecessor); ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); tempJoints.add(joint); predecessor = tempJoints.get(random.nextInt(tempJoints.size())).getSuccessor(); } PrismaticJoint[] jointArray = ScrewTools.filterJoints(ScrewTools.computeSubtreeJoints(rootBody), PrismaticJoint.class); return Arrays.asList(jointArray); }
public AfterJointReferenceFrameNameMap(RigidBody base) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(base); for(InverseDynamicsJoint joint : joints) { afterJointReferenceFrames.put(joint.getFrameAfterJoint().getName(), joint.getFrameAfterJoint()); } }
public static List<OneDoFJoint> createRandomTreeRobotWithOneDoFJoints(String prefix, RigidBody rootBody, int numberOfJoints, Random random) { List<OneDoFJoint> tempJoints = new ArrayList<>(); RigidBody predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint joint; if (random.nextBoolean()) joint = addRandomPrismaticJoint(prefix + "Joint" + i, random, predecessor); else joint = addRandomRevoluteJoint(prefix + "Joint" + i, random, predecessor); ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); tempJoints.add(joint); predecessor = tempJoints.get(random.nextInt(tempJoints.size())).getSuccessor(); } OneDoFJoint[] jointArray = ScrewTools.filterJoints(ScrewTools.computeSubtreeJoints(rootBody), OneDoFJoint.class); return Arrays.asList(jointArray); }
public SDFPerfectSimulatedSensorReader(FloatingRootJointRobot robot, FloatingInverseDynamicsJoint rootJoint, ForceSensorDataHolder forceSensorDataHolderToUpdate, ReferenceFrames referenceFrames) { name = robot.getName() + "SimulatedSensorReader"; this.robot = robot; this.referenceFrames = referenceFrames; this.forceSensorDataHolderToUpdate = forceSensorDataHolderToUpdate; this.rootJoint = rootJoint; InverseDynamicsJoint[] jointsArray = ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()); for (InverseDynamicsJoint joint : jointsArray) { if (joint instanceof OneDoFJoint) { OneDoFJoint oneDoFJoint = (OneDoFJoint) joint; String name = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = robot.getOneDegreeOfFreedomJoint(name); ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = new ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint>(oneDegreeOfFreedomJoint, oneDoFJoint); revoluteJoints.add(jointPair); } } }
/** * 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)); }
private void checkRobotMatchesData(String robotName, RigidBody rootBody, HSSFSheet descriptionSheet) { String robotNameInWorkbook = descriptionSheet.getRow(0).getCell(1).getStringCellValue(); if (!robotName.equals(robotNameInWorkbook)) { throw new RuntimeException("Trying to load the data for another robot: Loading data for " + robotName + ", workbook contains data for " + robotNameInWorkbook); } ArrayList<String> jointNames = new ArrayList<>(); int currentIndexValue = 2; HSSFRow currentRow = descriptionSheet.getRow(11); HSSFCell currentCell = currentRow.getCell(currentIndexValue); while (currentCell != null) { jointNames.add(currentCell.getStringCellValue()); currentCell = currentRow.getCell(currentIndexValue++); } InverseDynamicsJoint[] joints = ScrewTools.findJointsWithNames(ScrewTools.computeSubtreeJoints(rootBody), jointNames.toArray(new String[0])); OneDoFJoint[] oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); if (oneDoFJoints.length != jointNames.size()) { throw new RuntimeException("Could not find all the joints"); } }
public AntiGravityJointTorquesVisualizer(FullRobotModel fullRobotModel, TwistCalculator twistCalculator, SideDependentList<WrenchBasedFootSwitch> wrenchBasedFootSwitches, YoVariableRegistry parentRegistry, double gravity) { SpatialAccelerationVector rootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(twistCalculator.getRootBody(), gravity); this.inverseDynamicsCalculator = new InverseDynamicsCalculator(ReferenceFrame.getWorldFrame(), rootAcceleration, new LinkedHashMap<RigidBody, Wrench>(), new ArrayList<InverseDynamicsJoint>(), false, false, twistCalculator); this.wrenchBasedFootSwitches = wrenchBasedFootSwitches; allJoints = ScrewTools.computeSubtreeJoints(inverseDynamicsCalculator.getSpatialAccelerationCalculator().getRootBody()); allOneDoFJoints = ScrewTools.filterJoints(allJoints, OneDoFJoint.class); antiGravityJointTorques = new LinkedHashMap<>(allOneDoFJoints.length); for (int i = 0; i < allOneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint = allOneDoFJoints[i]; DoubleYoVariable antiGravityJointTorque = new DoubleYoVariable("antiGravity_tau_" + oneDoFJoint.getName(), registry); antiGravityJointTorques.put(oneDoFJoint, antiGravityJointTorque); } parentRegistry.addChild(registry); }
private void addJointHolders(List<RigidBody> rootBodies) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(rootBodies); for (InverseDynamicsJoint joint : joints) { JointHolder jointHolder = JointHolderFactory.getJointHolder(joint); YoProtoHandshake.JointDefinition.Builder jointDefinition = YoProtoHandshake.JointDefinition.newBuilder(); jointDefinition.setName(joint.getName()); jointDefinition.setType(jointHolder.getJointType()); yoProtoHandshakeBuilder.addJoint(jointDefinition); jointHolders.add(jointHolder); } }
private void addJointHolders(List<RigidBody> rootBodies) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(rootBodies); for (InverseDynamicsJoint joint : joints) { JointHolder jointHolder = JointHolderFactory.getJointHolder(joint); YoProtoHandshake.JointDefinition.Builder jointDefinition = YoProtoHandshake.JointDefinition.newBuilder(); jointDefinition.setName(joint.getName()); jointDefinition.setType(jointHolder.getJointType()); yoProtoHandshakeBuilder.addJoint(jointDefinition); jointHolders.add(jointHolder); } }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
public PositionStateRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionOutputPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityOutputPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionOutputPort = centerOfMassPositionOutputPort; this.centerOfMassVelocityOutputPort = centerOfMassVelocityOutputPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); }
private StateEstimatorSensorDefinitions buildStateEstimatorSensorDefinitions(FullRobotModel fullRobotModel) { InverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); ForceSensorDefinition[] forceSensorDefinitions = fullRobotModel.getForceSensorDefinitions(); StateEstimatorSensorDefinitions stateEstimatorSensorDefinitions = new StateEstimatorSensorDefinitions(); for (InverseDynamicsJoint joint : ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor())) { if (joint instanceof OneDoFJoint) { OneDoFJoint oneDoFJoint = (OneDoFJoint) joint; stateEstimatorSensorDefinitions.addJointSensorDefinition(oneDoFJoint); } } for (IMUDefinition imuDefinition : imuDefinitions) { stateEstimatorSensorDefinitions.addIMUSensorDefinition(imuDefinition); } for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) { stateEstimatorSensorDefinitions.addForceSensorDefinition(forceSensorDefinition); } return stateEstimatorSensorDefinitions; }
public OrientationAndPositionFullRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityPort, ControlFlowOutputPort<FrameVector> centerOfMassAccelerationPort, ControlFlowOutputPort<FrameOrientation> orientationPort, ControlFlowOutputPort<FrameVector> angularVelocityPort, ControlFlowOutputPort<FrameVector> angularAccelerationPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionPort = centerOfMassPositionPort; this.centerOfMassVelocityPort = centerOfMassVelocityPort; this.centerOfMassAccelerationPort = centerOfMassAccelerationPort; this.orientationPort = orientationPort; this.angularVelocityPort = angularVelocityPort; this.angularAccelerationPort = angularAccelerationPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); // TODO: Should pass the input port for the spatial acceleration calculator here too... this.centerOfMassAccelerationCalculator = new CenterOfMassAccelerationCalculator(rootJoint.getSuccessor(), ScrewTools.computeSupportAndSubtreeSuccessors(elevator), inverseDynamicsStructure.getSpatialAccelerationCalculator()); }