@Override public void initialize() { for (RobotSide robotSide : RobotSide.values) { for (ValkyrieHandJointName valkyrieHandJointName : ValkyrieHandJointName.values()) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = allHandJointsNameToJointsMap.get(robotSide).get(valkyrieHandJointName); currentHandJointAngles.get(robotSide).get(valkyrieHandJointName).set(oneDegreeOfFreedomJoint.getQ()); } } }
@Override public void write(long timestamp) { for (RobotSide robotSide : RobotSide.values) { for (ValkyrieHandJointName valkyrieHandJointName : ValkyrieHandJointName.values()) { double desiredQ = handJointFingerSetControllers.get(robotSide).getDesired(valkyrieHandJointName); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = allHandJointsNameToJointsMap.get(robotSide).get(valkyrieHandJointName); oneDegreeOfFreedomJoint.getQYoVariable().set(desiredQ); YoDouble currentHandJointAngle = currentHandJointAngles.get(robotSide).get(valkyrieHandJointName); currentHandJointAngle.set(oneDegreeOfFreedomJoint.getQ()); } EnumMap<ValkyrieHandJointName, YoDouble> handJointEnumMap = sideDependentHandJointHandlers.get(robotSide); for (ValkyrieHandJointName valkyrieHandJointName : ValkyrieHandJointName.values) { double desiredQ = handJointFingerSetControllers.get(robotSide).getDesired(valkyrieHandJointName); handJointEnumMap.get(valkyrieHandJointName).set(desiredQ); } EnumMap<ValkyrieFingerMotorName, YoDouble> fingerMotorEnumMap = sideDependentFingerMotorHandlers.get(robotSide); for (ValkyrieFingerMotorName valkyrieFingerMotorName : ValkyrieFingerMotorName.values) { double desiredQ = fingerSetControllers.get(robotSide).getDesired(valkyrieFingerMotorName); fingerMotorEnumMap.get(valkyrieFingerMotorName).set(desiredQ); } } }
break; case EFFORT: currentPosition = simulatedJoint.getQ(); desiredPosition = actuatorDesireds.isPositionValid() ? actuatorDesireds.getPosition() : currentPosition;
private void readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); if (pinJoint == null) continue; sensorOutputMap.setJointPositionProcessedOutput(revoluteJoint, pinJoint.getQ()); sensorOutputMap.setJointVelocityProcessedOutput(revoluteJoint, pinJoint.getQD()); sensorOutputMap.setJointAccelerationProcessedOutput(revoluteJoint, pinJoint.getQDD()); sensorOutputMap.setJointEnabled(revoluteJoint, pinJoint.isDynamic()); } }
joints[jointEnum.getIndex(robotSide)] = handJoints.get(robotSide).get(jointEnum).getQ();
joints[jointEnum.getIndex(robotSide)] = handJoints.get(robotSide).get(jointEnum).getQ();
oneDoFJointsB[jointIndex].setQ(oneDoFJointsA[jointIndex].getQ()); oneDoFJointsB[jointIndex].setQd(oneDoFJointsA[jointIndex].getQD()); oneDoFJointsB[jointIndex].setQdd(oneDoFJointsA[jointIndex].getQDD()); assertEquals(oneDoFJointsA[jointIndex].getQ(), oneDoFJointsB[jointIndex].getQ(), EPSILON); assertEquals(oneDoFJointsA[jointIndex].getQD(), oneDoFJointsB[jointIndex].getQD(), EPSILON); assertEquals(oneDoFJointsA[jointIndex].getQDD(), oneDoFJointsB[jointIndex].getQDD(), EPSILON);
double currentHandJoint = simulatedRobot.getOneDegreeOfFreedomJoint(valkyrieHandJointName.getJointName(robotSide)).getQ(); handJointHandler.set(currentHandJoint); sideDependentHandJointHandlers.get(robotSide).put(valkyrieHandJointName, handJointHandler);