public RigidBody getRigidBody(Joint joint) { if (joint instanceof FloatingJoint) { FloatingInverseDynamicsJoint parentSixDoFJoint = floatingToSixDofToJointMap.get(joint); return parentSixDoFJoint.getSuccessor(); } else if (joint instanceof OneDegreeOfFreedomJoint) { OneDoFJoint parentOneDoFJoint = scsToOneDoFJointMap.get(joint); return parentOneDoFJoint.getSuccessor(); } else { throw new RuntimeException(); } }
public ConstrainedCentroidalMomentumMatrixCalculator(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame centerOfMassFrame, DenseMatrix64F selectionMatrix) { this.dynamicallyConsistentNullspaceCalculator = new OriginalDynamicallyConsistentNullspaceCalculator(rootJoint, true); this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootJoint.getSuccessor(), centerOfMassFrame); this.selectionMatrix = selectionMatrix; }
public ConstrainedCenterOfMassJacobianCalculator(FloatingInverseDynamicsJoint rootJoint) { this.dynamicallyConsistentNullspaceCalculator = new OriginalDynamicallyConsistentNullspaceCalculator(rootJoint, true); this.centerOfMassJacobian = new CenterOfMassJacobian(rootJoint.getSuccessor()); }
public static InverseDynamicsJoint[] computeJointsToOptimizeFor(FullHumanoidRobotModel fullRobotModel, InverseDynamicsJoint... jointsToRemove) { List<InverseDynamicsJoint> joints = new ArrayList<InverseDynamicsJoint>(); InverseDynamicsJoint[] allJoints = ScrewTools.computeSupportAndSubtreeJoints(fullRobotModel.getRootJoint().getSuccessor()); joints.addAll(Arrays.asList(allJoints)); for (RobotSide robotSide : RobotSide.values) { RigidBody hand = fullRobotModel.getHand(robotSide); if (hand != null) { List<InverseDynamicsJoint> fingerJoints = Arrays.asList(ScrewTools.computeSubtreeJoints(hand)); joints.removeAll(fingerJoints); } } if (jointsToRemove != null) { for (InverseDynamicsJoint joint : jointsToRemove) { joints.remove(joint); } } return joints.toArray(new InverseDynamicsJoint[joints.size()]); }
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); } } }
public JointStateFullRobotModelUpdater(ControlFlowGraph controlFlowGraph, Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointPositionSensors, Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointVelocitySensors, FullInverseDynamicsStructure inverseDynamicsStructure) { InverseDynamicsJoint[] joints = ScrewTools.computeSupportAndSubtreeJoints(inverseDynamicsStructure.getRootJoint().getSuccessor()); this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); this.inverseDynamicsStructureOutputPort = createOutputPort("inverseDynamicsStructureOutputPort"); inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { if (jointPositionSensors.get(oneDoFJoint) == null) { throw new RuntimeException("positionSensorPorts.get(oneDoFJoint) == null. oneDoFJoint = " + oneDoFJoint); } } for (OneDoFJoint oneDoFJoint : oneDoFJoints) { ControlFlowOutputPort<double[]> positionSensorOutputPort = jointPositionSensors.get(oneDoFJoint); ControlFlowInputPort<double[]> positionSensorInputPort = createInputPort("positionSensorInputPort"); positionSensorInputPorts.put(oneDoFJoint, positionSensorInputPort); controlFlowGraph.connectElements(positionSensorOutputPort, positionSensorInputPort); ControlFlowOutputPort<double[]> velocitySensorOutputPort = jointVelocitySensors.get(oneDoFJoint); ControlFlowInputPort<double[]> velocitySensorInputPort = createInputPort("velocitySensorInputPort"); velocitySensorInputPorts.put(oneDoFJoint, velocitySensorInputPort); controlFlowGraph.connectElements(velocitySensorOutputPort, velocitySensorInputPort); } }
InverseDynamicsJoint[] inverseDynamicsJoints = ScrewTools.computeSubtreeJoints(sixDoFRootJoint.getSuccessor()); LinkedHashMap<String, OneDoFJoint> inverseDynamicsJointsByName = new LinkedHashMap<String, OneDoFJoint>();
public OriginalDynamicallyConsistentNullspaceCalculator(FloatingInverseDynamicsJoint rootJoint, boolean computeSNsBar) { this.rootJoint = rootJoint; this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); jointsInOrder = massMatrixCalculator.getJointsInOrder(); this.nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrixSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); lambdaSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); this.computeSNsBar = computeSNsBar; }
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()); }
public JointStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, SensorOutputMapReadOnly sensorOutputMapReadOnly, StateEstimatorParameters stateEstimatorParameters, YoVariableRegistry parentRegistry) { twistCalculator = inverseDynamicsStructure.getTwistCalculator(); spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); rootBody = twistCalculator.getRootBody(); this.sensorMap = sensorOutputMapReadOnly; InverseDynamicsJoint[] joints = ScrewTools.computeSupportAndSubtreeJoints(inverseDynamicsStructure.getRootJoint().getSuccessor()); this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); iMUBasedJointVelocityEstimator = createIMUBasedJointVelocityEstimator(sensorOutputMapReadOnly, stateEstimatorParameters, registry); parentRegistry.addChild(registry); }
public void build(FloatingInverseDynamicsJoint rootJoint, IMUDefinition[] imuDefinitions, ForceSensorDefinition[] forceSensorDefinitions, ContactSensorHolder contactSensorDataHolder, RawJointSensorDataHolderMap rawJointSensorDataHolderMap, DesiredJointDataHolder estimatorDesiredJointDataHolder, YoVariableRegistry registry) { 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); } atlasSensorReader = new GazeboSensorReader(stateEstimatorSensorDefinitions, sensorInformation, stateEstimatorParameters, rawJointSensorDataHolderMap, registry); }
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()); }
for (InverseDynamicsJoint joint : ScrewTools.computeSubtreeJoints(estimatorFullRobotModel.getRootJoint().getSuccessor()))
for (InverseDynamicsJoint joint : ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()))
private void computeRootJointAngularVelocity(TwistCalculator twistCalculator, FrameVector rootJointAngularVelocityToPack, FrameVector angularVelocityEstimationLink) { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); RigidBody estimationLink = inverseDynamicsStructure.getEstimationLink(); tempEstimationLinkAngularVelocity.setIncludingFrame(angularVelocityEstimationLink); // T_{root}^{root, estimation} twistCalculator.getRelativeTwist(tempRootToEstimationTwist, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationTwist.changeFrame(rootJoint.getFrameAfterJoint()); // omega_{root}^{root, estimation} tempRootToEstimationAngularVelocity.setToZero(rootJoint.getFrameAfterJoint()); tempRootToEstimationTwist.getAngularPart(tempRootToEstimationAngularVelocity); // omega_{estimation}^{root, world} tempEstimationLinkAngularVelocity.changeFrame(rootJoint.getFrameAfterJoint()); // omega_{root}^{root, world} = omega_{estimation}^{root, world} + omega_{root}^{root, estimation} rootJointAngularVelocityToPack.setToZero(rootJoint.getFrameAfterJoint()); rootJointAngularVelocityToPack.add(tempEstimationLinkAngularVelocity, tempRootToEstimationAngularVelocity); }
for (InverseDynamicsJoint joint : ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()))
private void updateRootJointTwistAngularPart() { // T_{rootBody}^{rootBody, measurementLink} twistCalculator.getRelativeTwist(twistRootJointFrameRelativeToMeasurementLink, measurementLink, rootJoint.getSuccessor()); // T_{rootBody}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeFrame(rootJointFrame); // T_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeBodyFrameNoRelativeTwist(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.getAngularPart(angularVelocityRootJointFrameRelativeToMeasurementLink); // omega_{measurementLink}^{measurementFrame, world} imuProcessedOutput.getAngularVelocityMeasurement(angularVelocityMeasurement); if (imuBiasProvider != null) { imuBiasProvider.getAngularVelocityBiasInIMUFrame(imuProcessedOutput, angularVelocityMeasurementBias); angularVelocityMeasurement.sub(angularVelocityMeasurementBias); } angularVelocityMeasurementLinkRelativeToWorld.setIncludingFrame(measurementFrame, angularVelocityMeasurement); // omega_{measurementLink}^{rootJointFrame, world} angularVelocityMeasurementLinkRelativeToWorld.changeFrame(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, world} = omega_{rootJointFrame}^{rootJointFrame, measurementLink} + omega_{measurementLink}^{rootJointFrame, world} angularVelocityRootJointFrameRelativeToWorld.add(angularVelocityRootJointFrameRelativeToMeasurementLink, angularVelocityMeasurementLinkRelativeToWorld); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setAngularPart(angularVelocityRootJointFrameRelativeToWorld); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); twistCalculator.compute(); yoRootJointAngularVelocity.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityMeasFrame.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityInWorld.setAndMatchFrame(angularVelocityRootJointFrameRelativeToWorld); }
twistCalculator.getRelativeTwist(tempRootToEstimationTwist, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationTwist.changeFrame(rootJoint.getFrameAfterJoint()); spatialAccelerationCalculator.getRelativeAcceleration(tempRootToEstimationAcceleration, estimationLink, rootJoint.getSuccessor()); tempRootToEstimationAcceleration.changeFrameNoRelativeMotion(rootJoint.getFrameAfterJoint()); tempRootToEstimationAcceleration.getAngularPart(tempRootToEstimationAngularAcceleration);
allJoints = ScrewTools.computeSupportAndSubtreeJoints(fullRobotModel.getRootJoint().getSuccessor()); v = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(allJoints), 1);
RigidBody rootBody = fullRobotModel.getRootJoint().getSuccessor(); SideDependentList<ContactablePlaneBody> handContactableBodies = contactableBodiesFactory.createHandContactableBodies(rootBody);