public void copy() { for(int i = 0; i < originalAndTarget.size(); i++) { RawJointSensorDataHolder original = originalAndTarget.first(i); RawJointSensorDataHolder target = originalAndTarget.second(i); target.set(original); } } }
@Override public void initialize() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJointBasics state = controlledJoints.first(i); jointTrajectories.get(state).initialize(state.getQ(), 0.0); } startTime.set(yoTime.getDoubleValue()); hasBeenInitialized = true; }
OneDoFJointBasics joint = controlledJoints.first(i); String jointName = joint.getName();
final OneDoFJointBasics jointState = jointStateAndData.first(i); final JointDesiredOutputBasics jointData = jointStateAndData.second(i);
OneDoFJointBasics jointState = jointStateAndData.first(i); YoDouble qd_d_joint = desiredVelocities.get(jointData); YoDouble q_d_joint = desiredPositions.get(jointData);
private void doIdleControl() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJointBasics state = controlledJoints.first(i); JointDesiredOutputBasics output = controlledJoints.second(i); PDController jointPDController = jointPDControllerMap.get(state); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(state); jointTrajectory.compute(trajectoryTimeProvider.getValue()); YoDouble jointDesiredPosition = jointDesiredPositionMap.get(state); YoDouble jointDesiredVelocity = jointDesiredVelocityMap.get(state); jointDesiredPosition.set(jointTrajectory.getValue()); jointDesiredVelocity.set(0.0); double q = state.getQ(); double qDesired = jointDesiredPosition.getDoubleValue(); double qd = state.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.clamp(qDesired - q, qdMax * controlDT); qdDesired = qd + MathTools.clamp(qdDesired - qd, qddMax * controlDT); tauDesired = MathTools.clamp(tauDesired, tauMax); output.setDesiredTorque(tauDesired); output.setDesiredPosition(qDesired); output.setDesiredVelocity(qdDesired); jointDesiredTauMap.get(state).set(tauDesired); } }
JointDesiredOutputBasics jointData = jointTorquesSmoothedAtStateChange.first(i); double tau = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0; AlphaFilteredYoVariable smoothedJointTorque = jointTorquesSmoothedAtStateChange.second(i);
OneDoFJointBasics state = controlledJoints.first(i); JointDesiredOutputBasics output = controlledJoints.second(i); PDController jointPDController = jointPDControllerMap.get(state);
@Override public void processAfterController(long timestamp) { for (int i = 0; i < torqueOffsetList.size(); i++) { JointDesiredOutputBasics jointData = torqueOffsetList.first(i); YoDouble torqueOffsetVariable = torqueOffsetList.second(i); double desiredAcceleration = jointData.hasDesiredAcceleration() ? jointData.getDesiredAcceleration() : 0.0; if (resetTorqueOffsets.getBooleanValue()) torqueOffsetVariable.set(0.0); double offsetTorque = torqueOffsetVariable.getDoubleValue(); double ditherTorque = 0.0; double alpha = alphaTorqueOffset.getDoubleValue(); offsetTorque = alpha * (offsetTorque + desiredAcceleration * updateDT) + (1.0 - alpha) * offsetTorque; torqueOffsetVariable.set(offsetTorque); double desiredTorque = jointData.hasDesiredTorque() ? jointData.getDesiredTorque() : 0.0; jointData.setDesiredTorque(desiredTorque + offsetTorque + ditherTorque); } if (drcOutputWriter != null) { drcOutputWriter.processAfterController(timestamp); } }
if (controlledJoints.first(i).getName().equals(jointToIgnore)) controlledJoints.remove(i); OneDoFJointBasics joint = controlledJoints.first(i); String name = joint.getName(); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = new OneDoFJointQuinticTrajectoryGenerator(name, joint, trajectoryTimeProvider, registry);
protected void write() { for (int i = 0; i < revoluteJoints.size(); i++) { OneDegreeOfFreedomJoint pinJoint = revoluteJoints.first(i); JointDesiredOutputReadOnly data = revoluteJoints.second(i); if(data.hasDesiredTorque()) { pinJoint.setTau(data.getDesiredTorque()); } if (data.hasStiffness()) { pinJoint.setKp(data.getStiffness()); } if (data.hasDamping()) { pinJoint.setKd(data.getDamping()); } if (data.hasDesiredPosition()) { pinJoint.setqDesired(data.getDesiredPosition()); } if (data.hasDesiredVelocity()) { pinJoint.setQdDesired(data.getDesiredVelocity()); } } }