public void setDesiredJointState(FloatingInverseDynamicsJoint rootJoint, OneDoFJoint[] newJointData) { if (newJointData.length != desiredJointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < desiredJointAngles.length; i++) desiredJointAngles[i] = (float) newJointData[i].getqDesired(); rootJoint.getTranslation(desiredRootTranslation); rootJoint.getRotation(desiredRootOrientation); }
public void packEstimatorJoints(long timestamp, long sensorHeadPPSTimestamp, RobotConfigurationData jointConfigurationData) { if (jointConfigurationData == null) { return; } rootJoint.getTranslation(rootTranslation); rootJoint.getRotation(rootOrientation); rootJoint.getAngularVelocity(rootAngularVelocity); rootJoint.getLinearVelocity(rootLinearVelocity); rootJoint.getLinearAcceleration(rootLinearAcceleration); jointConfigurationData.setPelvisAngularVelocity(rootAngularVelocity); jointConfigurationData.setPelvisLinearVelocity(rootLinearVelocity); jointConfigurationData.setPelvisLinearAcceleration(rootLinearAcceleration); jointConfigurationData.setRootTranslation(rootTranslation); jointConfigurationData.setRootOrientation(rootOrientation); jointConfigurationData.setJointState(joints); jointConfigurationData.setTimestamp(timestamp); jointConfigurationData.setSensorHeadPPSTimestamp(sensorHeadPPSTimestamp); for (int sensorNumber = 0; sensorNumber < getNumberOfForceSensors(); sensorNumber++) { float[] forceAndMomentVector = jointConfigurationData.getMomentAndForceVectorForSensor(sensorNumber); forceSensorDataList.get(sensorNumber).getWrench(forceAndMomentVector); } }
rootJoint.getTranslation(translation); estimatedRootJointPosition.set(translation);
PrintTools.debug("QuadTree has changed, sending packet"); Point3d rootJointPosition = new Point3d(); rootJoint.getTranslation(rootJointPosition); robotPosition2d.set(rootJointPosition.getX(), rootJointPosition.getY()); reportMessage(HeightQuadTreeMessageConverter.convertQuadTreeForGround(quadTree, robotPosition2d, quadTreeMessageMaxRadius));