public static void setRandomAccelerations(Iterable<? extends OneDoFJoint> joints, Random random) { for (OneDoFJoint joint : joints) { joint.setQdd(random.nextDouble()); } }
revoluteJoint.setQddDesired(jointAcceleration); else revoluteJoint.setQdd(jointAcceleration);
public static void setRandomAccelerations(OneDoFJoint[] joints, Random random) { for (OneDoFJoint joint : joints) { joint.setQdd(random.nextDouble()); } }
public static void copyDesiredAccelerationsToActual(Iterable<? extends OneDoFJoint> joints) { for (OneDoFJoint joint : joints) { joint.setQdd(joint.getQddDesired()); } }
public static void setJointAccelerations(OneDoFJoint[] jointList, DenseMatrix64F jointAccelerations) { int rowStart = 0; for (OneDoFJoint joint : jointList) { joint.setQdd(jointAccelerations.get(rowStart, 0)); rowStart += joint.getDegreesOfFreedom(); } }
private void readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); if (pinJoint == null) continue; revoluteJoint.setQ(pinJoint.getQYoVariable().getDoubleValue()); revoluteJoint.setQd(pinJoint.getQDYoVariable().getDoubleValue()); revoluteJoint.setQdd(pinJoint.getQDDYoVariable().getDoubleValue()); } }
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); }
@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint); setQ(oneDoFOriginalJoint.getQ()); setQd(oneDoFOriginalJoint.getQd()); setQdd(oneDoFOriginalJoint.getQdd()); setTauMeasured(oneDoFOriginalJoint.getTauMeasured()); setEnabled(oneDoFOriginalJoint.isEnabled()); }
joint.setQdd(qDotDotDesired);