How to use

InverseKinematicsCommandList

in

us.ihmc.commonWalkingControlModules.controllerCore.command.inverseKinematics

@Override
public void set(InverseKinematicsCommandList other)
{
 clear();
 for (int i = 0; i < other.getNumberOfCommands(); i++)
   addCommand(other.getCommand(i));
}

@Override
protected InverseKinematicsCommandList getAdditionalInverseKinematicsCommands()
{
 InverseKinematicsCommandList commands = new InverseKinematicsCommandList();
 commands.addCommand(createJointLimitReductionCommand());
 return commands;
}

private void submitInverseKinematicsCommandList(InverseKinematicsCommandList command)
{
 while (command.getNumberOfCommands() > 0)
   submitInverseKinematicsCommand(command.pollCommand());
}

/**
* Adds command to be considered by the inverse kinematics controller core.
* @param inverseKinematicsCommand command to add to the list
*/
public void addInverseKinematicsCommand(InverseKinematicsCommand<?> inverseKinematicsCommand)
{
 if (inverseKinematicsCommand != null)
   inverseKinematicsCommandList.addCommand(inverseKinematicsCommand);
}

public void copyFromOther(InverseKinematicsCommandList other)
{
 clear();
 for (int i = 0; i < other.getNumberOfCommands(); i++)
 {
   InverseKinematicsCommand<?> commandToCopy = other.getCommand(i);
   switch (commandToCopy.getCommandType())
   {
   case JOINTSPACE:
    copyJointspaceVelocityCommand((JointspaceVelocityCommand) commandToCopy);
    break;
   case MOMENTUM:
    copyMomentumCommand((MomentumCommand) commandToCopy);
    break;
   case TASKSPACE:
    copySpatialVelocityCommand((SpatialVelocityCommand) commandToCopy);
   default:
    throw new RuntimeException("The command type: " + commandToCopy.getCommandType() + " is not handled.");
   }
 }
}

/**
* Clears all the command lists.
*/
public void clear()
{
 inverseDynamicsCommandList.clear();
 feedbackControlCommandList.clear();
 inverseKinematicsCommandList.clear();
 lowLevelOneDoFJointDesiredDataHolder.clear();
}

/**
* Class that contains the different lists that are submitted the whole body controller core.
* All desired behaviors to be considered must be submitted to the whole body controller core
* through this command.
* @param controllerCoreMode desired controller core mode. Choose between Inverse Dynamics, Inverse Kinematics, or Virtual Model Control.
*/
public ControllerCoreCommand(WholeBodyControllerCoreMode controllerCoreMode)
{
 this.controllerCoreMode = controllerCoreMode;
 inverseDynamicsCommandList = new InverseDynamicsCommandList();
 virtualModelControlCommandList = new InverseDynamicsCommandList();
 feedbackControlCommandList = new FeedbackControlCommandList();
 inverseKinematicsCommandList = new InverseKinematicsCommandList();
 lowLevelOneDoFJointDesiredDataHolder = new LowLevelOneDoFJointDesiredDataHolder();
}

public InverseKinematicsCommand<?> pollCommand()
{
 if (commandList.isEmpty())
   return null;
 else
   return commandList.remove(getNumberOfCommands() - 1);
}

private void copyJointspaceVelocityCommand(JointspaceVelocityCommand commandToCopy)
{
 JointspaceVelocityCommand localCommand = jointspaceVelocityCommands.add();
 localCommand.set(commandToCopy);
 inverseKinematicsCommandList.addCommand(localCommand);
}

public void copyFromOther(InverseKinematicsCommandList other)
{
 clear();
 for (int i = 0; i < other.getNumberOfCommands(); i++)
 {
   InverseKinematicsCommand<?> commandToCopy = other.getCommand(i);
   switch (commandToCopy.getCommandType())
   {
   case JOINTSPACE:
    copyJointspaceVelocityCommand((JointspaceVelocityCommand) commandToCopy);
    break;
   case MOMENTUM:
    copyMomentumCommand((MomentumCommand) commandToCopy);
    break;
   case TASKSPACE:
    copySpatialVelocityCommand((SpatialVelocityCommand) commandToCopy);
   default:
    throw new RuntimeException("The command type: " + commandToCopy.getCommandType() + " is not handled.");
   }
 }
}

  private void clear()
  {
   inverseKinematicsCommandList.clear();
   jointspaceVelocityCommands.clear();
   spatialVelocityCommands.clear();
   momentumCommands.clear();
  }
}

private InverseKinematicsCommandList computeInverseKinematicsCommands()
 InverseKinematicsCommandList ret = new InverseKinematicsCommandList();
    spatialVelocityCommand.set(desiredHandTwist, selectionMatrix);
    spatialVelocityCommand.setWeight(handWeight.getDoubleValue());
    ret.addCommand(spatialVelocityCommand);
    spatialVelocityCommand.set(desiredFootTwist, selectionMatrix);
    spatialVelocityCommand.setWeight(footWeight.getDoubleValue());
    ret.addCommand(spatialVelocityCommand);
   momentumCommand.setLinearMomentumXY(desiredMomentumXY);
   momentumCommand.setWeight(momentumWeight.getDoubleValue());
   ret.addCommand(momentumCommand);
   spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity);
   spatialVelocityCommand.setWeight(chestWeight.getDoubleValue());
   ret.addCommand(spatialVelocityCommand);
   spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity);
   spatialVelocityCommand.setWeight(pelvisOrientationWeight.getDoubleValue());
   ret.addCommand(spatialVelocityCommand);
 ret.addCommand(privilegedConfigurationCommandReference.getAndSet(null));
 ret.addCommand(jointLimitReductionCommand);

private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy)
{
 SpatialVelocityCommand localCommand = spatialVelocityCommands.add();
 localCommand.set(commandToCopy);
 inverseKinematicsCommandList.addCommand(localCommand);
}

  private void clear()
  {
   inverseKinematicsCommandList.clear();
   jointspaceVelocityCommands.clear();
   spatialVelocityCommands.clear();
   momentumCommands.clear();
  }
}

private InverseKinematicsCommandList computeInverseKinematicsCommands()
 InverseKinematicsCommandList ret = new InverseKinematicsCommandList();
    spatialVelocityCommand.set(desiredHandTwist, selectionMatrix);
    spatialVelocityCommand.setWeight(handWeight.getDoubleValue());
    ret.addCommand(spatialVelocityCommand);
    spatialVelocityCommand.set(desiredFootTwist, selectionMatrix);
    spatialVelocityCommand.setWeight(footWeight.getDoubleValue());
    ret.addCommand(spatialVelocityCommand);
   momentumCommand.setLinearMomentumXY(desiredMomentumXY);
   momentumCommand.setWeight(momentumWeight.getDoubleValue());
   ret.addCommand(momentumCommand);
   spatialVelocityCommand.set(desiredChestTwist, chestSelectionMatrix);
   spatialVelocityCommand.setWeight(chestWeight.getDoubleValue());
   ret.addCommand(spatialVelocityCommand);
   spatialVelocityCommand.set(desiredPelvisTwist, pelvisSelectionMatrix);
   spatialVelocityCommand.setWeight(pelvisOrientationWeight.getDoubleValue());
   ret.addCommand(spatialVelocityCommand);
 ret.addCommand(privilegedConfigurationCommandReference.getAndSet(null));
 ret.addCommand(jointLimitReductionCommand);

private void copyMomentumCommand(MomentumCommand commandToCopy)
{
 MomentumCommand localCommand = momentumCommands.add();
 localCommand.set(commandToCopy);
 inverseKinematicsCommandList.addCommand(localCommand);
}

private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy)
{
 SpatialVelocityCommand localCommand = spatialVelocityCommands.add();
 localCommand.set(commandToCopy);
 inverseKinematicsCommandList.addCommand(localCommand);
}

private void copyJointspaceVelocityCommand(JointspaceVelocityCommand commandToCopy)
{
 JointspaceVelocityCommand localCommand = jointspaceVelocityCommands.add();
 localCommand.set(commandToCopy);
 inverseKinematicsCommandList.addCommand(localCommand);
}

private void copyMomentumCommand(MomentumCommand commandToCopy)
{
 MomentumCommand localCommand = momentumCommands.add();
 localCommand.set(commandToCopy);
 inverseKinematicsCommandList.addCommand(localCommand);
}