public static void setDesiredJointPositions(OneDoFJoint[] joints, DenseMatrix64F jointPositions) { int rowStart = 0; for (OneDoFJoint joint : joints) { joint.setqDesired(jointPositions.get(rowStart, 0)); rowStart += joint.getDegreesOfFreedom(); } }
private void doIdleControl() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJoint joint = controlledJoints.get(i); PDController jointPDController = jointPDControllerMap.get(joint); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(joint); jointTrajectory.compute(trajectoryTimeProvider.getValue()); DoubleYoVariable jointDesiredPosition = jointDesiredPositionMap.get(joint); DoubleYoVariable jointDesiredVelocity = jointDesiredVelocityMap.get(joint); jointDesiredPosition.set(jointTrajectory.getValue()); jointDesiredVelocity.set(0.0); double q = joint.getQ(); double qDesired = jointDesiredPosition.getDoubleValue(); double qd = joint.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired); double qdMax = qdMaxIdle.getDoubleValue(); double qddMax = qddMaxIdle.getDoubleValue(); double tauMax = tauMaxIdle.getDoubleValue(); qDesired = q + MathTools.clipToMinMax(qDesired - q, qdMax * controlDT); qdDesired = qd + MathTools.clipToMinMax(qdDesired - qd, qddMax * controlDT); tauDesired = MathTools.clipToMinMax(tauDesired, tauMax); joint.setTau(tauDesired); joint.setqDesired(qDesired); joint.setQdDesired(qdDesired); jointDesiredTauMap.get(joint).set(tauDesired); } }
double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired) + desiredJointTauOffset; joint.setTau(tauDesired); joint.setqDesired(qDesired); joint.setQdDesired(qdDesired);
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)); } } }
private void parseLowLevelDataInOneDoFJoints() { for (int i = 0; i < controlledOneDoFJoints.length; i++) { OneDoFJoint joint = controlledOneDoFJoints[i]; LowLevelJointDataReadOnly lowLevelJointData = yoLowLevelOneDoFJointDesiredDataHolder.getLowLevelJointData(joint); if (!lowLevelJointData.hasControlMode()) throw new NullPointerException("Joint: " + joint.getName() + " has no control mode."); switch (lowLevelJointData.getControlMode()) { case FORCE_CONTROL: joint.setUnderPositionControl(false); break; case POSITION_CONTROL: joint.setUnderPositionControl(true); break; default: throw new RuntimeException("Unhandled joint control mode: " + lowLevelJointData.getControlMode()); } if (lowLevelJointData.hasDesiredPosition()) joint.setqDesired(lowLevelJointData.getDesiredPosition()); if (lowLevelJointData.hasDesiredVelocity()) joint.setQdDesired(lowLevelJointData.getDesiredVelocity()); if (lowLevelJointData.hasDesiredAcceleration()) joint.setQddDesired(lowLevelJointData.getDesiredAcceleration()); if (lowLevelJointData.hasDesiredTorque()) joint.setTau(lowLevelJointData.getDesiredTorque()); } }