/** * 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); }
private void copyJointspaceVelocityCommand(JointspaceVelocityCommand commandToCopy) { JointspaceVelocityCommand localCommand = jointspaceVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy) { SpatialVelocityCommand localCommand = spatialVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
@Override protected InverseKinematicsCommandList getAdditionalInverseKinematicsCommands() { InverseKinematicsCommandList commands = new InverseKinematicsCommandList(); commands.addCommand(createJointLimitReductionCommand()); return commands; }
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); }
@Override public void set(InverseKinematicsCommandList other) { clear(); for (int i = 0; i < other.getNumberOfCommands(); i++) addCommand(other.getCommand(i)); }
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);
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);