public static void setRandomVelocity(OneDoFJoint joint, Random random) { joint.setQd(random.nextDouble()); }
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()); } }
double jointAcceleration = pinJoint.getQDDYoVariable().getDoubleValue(); revoluteJoint.setQ(jointPosition); revoluteJoint.setQd(jointVelocity);
public static void setRandomVelocity(OneDoFJoint joint, Random random, double min, double max) { joint.setQd(RandomTools.generateRandomDouble(random, min, max)); }
@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()); }
@Override public void setVelocity(DenseMatrix64F matrix, int rowStart) { setQd(matrix.get(rowStart, 0)); }
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 static void integrateAccelerations(Iterable<? extends OneDoFJoint> joints, double dt) { for (OneDoFJoint joint : joints) { joint.setQd(joint.getQd() + joint.getQdd() * dt); } }
joint.setQd(qDotDesired); joint.setQdd(qDotDotDesired);
standPrepJoint.setQd(rawSensorData.getQd_raw());
standPrepJoint.setQd(rawSensorData.getQd_raw());
oneDoFJoint.setQd(velocitySensorData); oneDoFJoint.setTauMeasured(torqueSensorData);
private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
oneDoFJoint.setQd(jointState.getQd()); state.updateRawSensorData(joint, rawSensor);
oneDoFJoint.setQd(jointState.getQd()); state.updateRawSensorData(joint, rawSensor);
oneDoFJoints[i].setQd(0.0);
oneDoFJoints[i].setQd(0.0);