public InverseDynamicsCalculator(ReferenceFrame inertialFrame, SpatialAccelerationVector rootAcceleration, HashMap<RigidBody, Wrench> externalWrenches, List<InverseDynamicsJoint> jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms, TwistCalculator twistCalculator) { this(inertialFrame, externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(twistCalculator.getRootBody(), inertialFrame, rootAcceleration, twistCalculator, doVelocityTerms, doAccelerationTerms, true), twistCalculator, doVelocityTerms); }
public InverseDynamicsCalculator(ReferenceFrame inertialFrame, HashMap<RigidBody, Wrench> externalWrenches, List<InverseDynamicsJoint> jointsToIgnore, SpatialAccelerationCalculator spatialAccelerationCalculator, TwistCalculator twistCalculator, boolean doVelocityTerms) { this.rootBody = twistCalculator.getRootBody(); this.externalWrenches = new LinkedHashMap<RigidBody, Wrench>(externalWrenches); this.jointsToIgnore = new ArrayList<InverseDynamicsJoint>(jointsToIgnore); this.twistCalculator = twistCalculator; this.spatialAccelerationCalculator = spatialAccelerationCalculator; this.doVelocityTerms = doVelocityTerms; populateMapsAndLists(); }
public MomentumCalculator(TwistCalculator twistCalculator) { this(twistCalculator, ScrewTools.computeSupportAndSubtreeSuccessors(twistCalculator.getRootBody())); }
public InverseDynamicsCalculator(TwistCalculator twistCalculator, double gravity, List<InverseDynamicsJoint> jointsToIgnore) { this(ReferenceFrame.getWorldFrame(), ScrewTools.createGravitationalSpatialAcceleration(twistCalculator.getRootBody(), gravity), new LinkedHashMap<RigidBody, Wrench>(), jointsToIgnore, true, true, twistCalculator); }
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); }
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); }
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 startComputation() { for (OneDoFJoint joint : oneDoFJoints) { if (joint == null) throw new RuntimeException(); ControlFlowInputPort<double[]> positionSensorPort = positionSensorInputPorts.get(joint); ControlFlowInputPort<double[]> velocitySensorPort = velocitySensorInputPorts.get(joint); double positionSensorData = positionSensorPort.getData()[0]; double velocitySensorData = velocitySensorPort.getData()[0]; joint.setQ(positionSensorData); joint.setQd(velocitySensorData); joint.setQdd(joint.getQddDesired()); } // TODO: Does it make sense to do this yet if the orientation of the pelvis isn't known yet? FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureOutputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); twistCalculator.getRootBody().updateFramesRecursively(); twistCalculator.compute(); spatialAccelerationCalculator.compute(); inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure); }
public MotionQPInputCalculator(ReferenceFrame centerOfMassFrame, GeometricJacobianHolder geometricJacobianHolder, TwistCalculator twistCalculator, JointIndexHandler jointIndexHandler, JointPrivilegedConfigurationParameters jointPrivilegedConfigurationParameters, YoVariableRegistry parentRegistry) { this.geometricJacobianHolder = geometricJacobianHolder; this.jointIndexHandler = jointIndexHandler; this.jointsToOptimizeFor = jointIndexHandler.getIndexedJoints(); oneDoFJoints = jointIndexHandler.getIndexedOneDoFJoints(); pointJacobianConvectiveTermCalculator = new PointJacobianConvectiveTermCalculator(twistCalculator); centroidalMomentumHandler = new CentroidalMomentumHandler(twistCalculator.getRootBody(), centerOfMassFrame, registry); privilegedConfigurationHandler = new JointPrivilegedConfigurationHandler(oneDoFJoints, jointPrivilegedConfigurationParameters, registry); numberOfDoFs = jointIndexHandler.getNumberOfDoFs(); allTaskJacobian = new DenseMatrix64F(numberOfDoFs, numberOfDoFs); secondaryTaskJointsWeight.set(1.0); // TODO Needs to be rethought, it doesn't seem to be that useful. nullspaceProjectionAlpha.set(0.005); nullspaceCalculator = new DampedLeastSquaresNullspaceCalculator(numberOfDoFs, nullspaceProjectionAlpha.getDoubleValue()); parentRegistry.addChild(registry); }
centroidalMomentumRateCalculator = new CentroidalMomentumRateCalculator(twistCalculator.getRootBody(), toolbox.getCenterOfMassFrame());
public AbstractFootControlState(ConstraintType stateEnum, FootControlHelper footControlHelper) { super(stateEnum); this.footControlHelper = footControlHelper; this.contactableFoot = footControlHelper.getContactableFoot(); this.momentumBasedController = footControlHelper.getMomentumBasedController(); this.robotSide = footControlHelper.getRobotSide(); rootBody = momentumBasedController.getTwistCalculator().getRootBody(); FullHumanoidRobotModel fullRobotModel = footControlHelper.getMomentumBasedController().getFullRobotModel(); RigidBody pelvis = fullRobotModel.getPelvis(); RigidBody foot = fullRobotModel.getFoot(robotSide); OneDoFJoint kneeJoint = fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH); straightLegsPrivilegedConfigurationCommand.addJoint(kneeJoint, PrivilegedConfigurationOption.AT_ZERO); straightLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, foot); bentLegsPrivilegedConfigurationCommand.addJoint(kneeJoint, PrivilegedConfigurationOption.AT_MID_RANGE); bentLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, foot); }
RigidBody elevator = twistCalculator.getRootBody(); ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); twistCalculator.getRelativeTwist(twistOfMeasurementLink, elevator, measurementLink);