/** ID Robot --> SCS Robot Copy the torques from the IDRobot to the SCSRobot. */
 public void updateTorquesSCSrobot() // Remember! the body joint is NOT actuated
     {
   for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
     OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
     scsJoint.setTau(idJoint.getTau());
   }
 }
  /** SCS Robot --> ID Robot Send positions and velocities. */
  public void updatePositionsIDrobot() {
    // Body info
    bodyJointID.setPosition(
        bodyJointSCS.getQ_t1().getDoubleValue(), 0.0, bodyJointSCS.getQ_t2().getDoubleValue());
    bodyJointID.setRotation(0.0, bodyJointSCS.getQ_rot().getDoubleValue(), 0.0);

    double[] velocityArray = new double[6];
    velocityArray[0] = 0.0; // yaw
    velocityArray[1] = bodyJointSCS.getQd_rot().getDoubleValue(); // pitch
    velocityArray[2] = 0.0; // roll
    velocityArray[3] = bodyJointSCS.getQd_t1().getDoubleValue(); // x
    velocityArray[4] = 0.0; // y
    velocityArray[5] = bodyJointSCS.getQd_t2().getDoubleValue(); // z
    DenseMatrix64F velocities = new DenseMatrix64F(6, 1, true, velocityArray);
    bodyJointID.setVelocity(velocities, 0);

    // Leg  and foot info (hip, knee and ankle)
    for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
      OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
      idJoint.setQ(scsJoint.getQYoVariable().getDoubleValue());
      idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue());
    }

    elevator.updateFramesRecursively();

    // Get the body coordinates
    bodyPosition = new FramePoint();
    bodyPosition.setToZero(bodyJointID.getFrameAfterJoint());
    bodyPosition.changeFrame(worldFrame);
  }
  public void startComputation() {
    for (OneDoFJoint joint : oneDoFJoints) {
      if (joint == null) throw new RuntimeException();

      ControlFlowInputPort<double[]> positionSensorPort = positionSensorInputPorts.get(joint);
      ControlFlowInputPort<double[]> velocitySensorPort = velocitySensorInputPorts.get(joint);

      double positionSensorData = positionSensorPort.getData()[0];
      double velocitySensorData = velocitySensorPort.getData()[0];

      joint.setQ(positionSensorData);
      joint.setQd(velocitySensorData);
      joint.setQdd(joint.getQddDesired());
    }

    // TODO: Does it make sense to do this yet if the orientation of the pelvis isn't known yet?
    FullInverseDynamicsStructure inverseDynamicsStructure =
        inverseDynamicsStructureOutputPort.getData();

    TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator();
    SpatialAccelerationCalculator spatialAccelerationCalculator =
        inverseDynamicsStructure.getSpatialAccelerationCalculator();

    twistCalculator.getRootBody().updateFramesRecursively();
    twistCalculator.compute();
    spatialAccelerationCalculator.compute();

    inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure);
  }
  @Override
  public void write() {
    for (int i = 0; i < revoluteJoints.size(); i++) {
      ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
      OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
      OneDoFJoint revoluteJoint = jointPair.getRight();

      pinJoint.setTau(revoluteJoint.getTau());
    }
  }
  public void updateRobotConfigurationBasedOnFullRobotModel() {
    for (int i = 0; i < revoluteJoints.size(); i++) {
      ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i);
      OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
      OneDoFJoint revoluteJoint = jointPair.getRight();

      pinJoint.setQ(revoluteJoint.getQ());
    }

    FloatingJoint floatingJoint = rootJointPair.getLeft();
    FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight();

    RigidBodyTransform transform = sixDoFJoint.getJointTransform3D();
    floatingJoint.setRotationAndTranslation(transform);
  }
  @Override
  public void setFullRobotModel(FullRobotModel fullRobotModel) {
    revoluteJoints.clear();
    OneDoFJoint[] revoluteJointsArray = fullRobotModel.getOneDoFJoints();

    for (OneDoFJoint revoluteJoint : revoluteJointsArray) {
      String name = revoluteJoint.getName();
      OneDegreeOfFreedomJoint oneDoFJoint = robot.getOneDegreeOfFreedomJoint(name);

      ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair =
          new ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint>(oneDoFJoint, revoluteJoint);
      this.revoluteJoints.add(jointPair);
    }

    rootJointPair =
        new ImmutablePair<FloatingJoint, FloatingInverseDynamicsJoint>(
            robot.getRootJoint(), fullRobotModel.getRootJoint());
  }
  @Override
  public void writeAfterController(long timestamp) {
    jointCommand.clear();

    jointCommand.putLong(estimatorTicksPerControlTick);
    jointCommand.putLong(timestamp);
    jointCommand.putLong(estimatorFrequencyInHz);

    for (int i = 0; i < joints.size(); i++) {
      OneDoFJoint joint = joints.get(i);

      if (fingerJointMap == null || !fingerJointMap.containsKey(joint.getName())) {
        if (joint.isUnderPositionControl()) jointCommand.putDouble(joint.getqDesired());
        else jointCommand.putDouble(joint.getTau());
      } else
        jointCommand.putDouble(
            fingerJointMap
                .get(joint.getName())
                .getqDesired()); // fingers are always position controlled
    }

    jointCommand.flip();

    try {
      while (jointCommand.hasRemaining()) {
        channel.write(jointCommand);
      }
    } catch (IOException e) {
      e.printStackTrace();
    }
  }
  public void process(long timestamp) {
    for (WandererJoint joint : WandererJoint.values) {
      AcsellJointState jointState = state.getJointState(joint);
      OneDoFJoint oneDoFJoint = jointMap.get(joint);
      RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint);

      oneDoFJoint.setQ(jointState.getQ());
      oneDoFJoint.setQd(jointState.getQd());
      state.updateRawSensorData(joint, rawSensor);
    }

    AcsellXSensState xSensState = state.getXSensState();
    xSensState.getQuaternion(rotation);
    //      rootJoint.setRotation(rotation);
    rootJoint.updateFramesRecursively();
    if (!initialized) {
      controller.initialize(timestamp);
      initialized = true;
    }

    controller.doControl(timestamp);

    if (controller.turnOutputOn()) {
      command.enableActuators();
    }

    for (WandererJoint joint : WandererJoint.values) {
      OneDoFJoint oneDoFJoint = jointMap.get(joint);
      RawJointSensorDataHolder rawSensor = rawSensors.get(oneDoFJoint);

      AcsellJointCommand jointCommand = command.getAcsellJointCommand(joint);
      jointCommand.setTauDesired(oneDoFJoint.getTau(), 0.0, rawSensor);
      jointCommand.setDamping(oneDoFJoint.getKd());
    }

    outputWriter.write();

    if (visualizer != null) {
      visualizer.update(timestamp);
    }
  }
 public double getValue() {
   return joint.getQ();
 }