public CenterOfMassAccelerationCalculator(RigidBody rootBody, SpatialAccelerationCalculator spatialAccelerationCalculator) { this(rootBody, ScrewTools.computeSupportAndSubtreeSuccessors(rootBody), spatialAccelerationCalculator); }
public CenterOfMassCalculator(RigidBody rootBody, ReferenceFrame desiredFrame) { this(ScrewTools.computeSupportAndSubtreeSuccessors(rootBody), desiredFrame); //TODO: This gets too much stuff. Shouldn't it just get the rootBody and everything posterior to it? // this(ScrewTools.computeSubtreeSuccessors(rootBody.getParentJoint()), desiredFrame); //TODO: Should it be something like this instead? }
public CenterOfMassJacobian(RigidBody rootBody) { this(ScrewTools.computeSupportAndSubtreeSuccessors(rootBody), rootBody.getBodyFixedFrame()); }
public MomentumCalculator(TwistCalculator twistCalculator) { this(twistCalculator, ScrewTools.computeSupportAndSubtreeSuccessors(twistCalculator.getRootBody())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeSupportAndSubtreeSuccessors_RigidBody() { int numberOfBodiesOnChain = 6; int numberOfBodies = 16; RigidBodyBasics[] successors = ScrewTools.computeSupportAndSubtreeSuccessors(secondLevelSubTrees.get(0)); assertEquals(numberOfBodiesOnChain - 1, successors.length); successors = ScrewTools.computeSupportAndSubtreeSuccessors(elevator); assertEquals(numberOfBodies - 1, successors.length); }
public RigidBodyToIndexMap(RigidBody rootBody) { rigidBodies = ScrewTools.computeSupportAndSubtreeSuccessors(rootBody); rigidBodyNames = new String[rigidBodies.length]; for (int index = 0; index < rigidBodies.length; index++) { RigidBody rigidBody = rigidBodies[index]; rigidBodyNames[index] = rigidBody.getName(); rigidBodyIndexByNameMap.put(rigidBody.getName(), index); rigidBodyByNameMap.put(rigidBody.getName(), rigidBody); } }
public ExternalWrenchHandler(double gravityZ, ReferenceFrame centerOfMassFrame, InverseDynamicsJoint rootJoint, List<? extends ContactablePlaneBody> contactablePlaneBodies) { this.centerOfMassFrame = centerOfMassFrame; MathTools.checkIfInRange(gravityZ, 0.0, Double.POSITIVE_INFINITY); this.contactablePlaneBodies = new ArrayList<>(contactablePlaneBodies); gravitationalWrench = new SpatialForceVector(centerOfMassFrame); double totalMass = TotalMassCalculator.computeMass(ScrewTools.computeSupportAndSubtreeSuccessors(rootJoint.getSuccessor())); gravitationalWrench.setLinearPartZ(-gravityZ * totalMass); totalWrenchAlreadyApplied = new SpatialForceVector(centerOfMassFrame); for (int i = 0; i < contactablePlaneBodies.size(); i++) { RigidBody rigidBody = this.contactablePlaneBodies.get(i).getRigidBody(); externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); } }
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 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()); }