@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); }