@Override public void setFullRobotModel(FullRobotModel fullRobotModel) { for (StepprJoint joint : StepprJoint.values) { joints.put(joint, fullRobotModel.getOneDoFJointByName(joint.getSdfName())); controllers.put(joint, new PDController(joint.getSdfName(), registry)); } for (StepprStandPrepSetpoints setpoint : StepprStandPrepSetpoints.values) { for (int i = 0; i < setpoint.getJoints().length; i++) { StepprJoint joint = setpoint.getJoints()[i]; YoDouble desiredPosition = new YoDouble(setpoint.getName() + "_q_d", registry); desiredPosition.set(setpoint.getQ()); if (i == 1) { desiredPosition.mul(setpoint.getReflectRight()); } desiredOffsets.put(joint, desiredPosition); controllers.get(joint).setProportionalGain(setpoint.getKp()); controllers.get(joint).setProportionalGain(setpoint.getKd()); joints.get(joint).setKd(setpoint.getDamping()); } } }
for (int i = 0; i < jointsToWiggleLists.get(axis).size(); i++) meanOfJointVelocities.get(axis).add(jointsToWiggleLists.get(axis).get(i).getQd()); meanOfJointVelocities.get(axis).mul(-1.0 / jointsToWiggleLists.get(axis).size());