private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy) { SpatialVelocityCommand localCommand = spatialVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, hand); spatialVelocityCommand.set(desiredHandTwist, selectionMatrix); spatialVelocityCommand.setWeight(handWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, foot); spatialVelocityCommand.set(desiredFootTwist, selectionMatrix); spatialVelocityCommand.setWeight(footWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); desiredChestTwist.getAngularVelocityInBaseFrame(desiredChestAngularVelocity); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, chest); spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity); spatialVelocityCommand.set(desiredChestTwist, chestSelectionMatrix); spatialVelocityCommand.setWeight(chestWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); desiredPelvisTwist.getAngularVelocityInBaseFrame(desiredPelvisAngularVelocity); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, pelvis); spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity); spatialVelocityCommand.set(desiredPelvisTwist, pelvisSelectionMatrix); spatialVelocityCommand.setWeight(pelvisOrientationWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand);
newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, hand); spatialVelocityCommand.set(desiredHandTwist, selectionMatrix); spatialVelocityCommand.setWeight(handWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, foot); spatialVelocityCommand.set(desiredFootTwist, selectionMatrix); spatialVelocityCommand.setWeight(footWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); FrameVector desiredChestAngularVelocity = computeDesiredAngularVelocity(desiredChestOrientation, chestFrame); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, chest); spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity); spatialVelocityCommand.setWeight(chestWeight.getDoubleValue()); FrameVector desiredPelvisAngularVelocity = computeDesiredAngularVelocity(desiredPelvisOrientation, pelvisFrame); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, pelvis); spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity); spatialVelocityCommand.setWeight(pelvisOrientationWeight.getDoubleValue());
private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy) { SpatialVelocityCommand localCommand = spatialVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
public void retrieveRigidBodiesFromName(Map<String, RigidBodyBasics> nameToRigidBodyMap) { for (int i = 0; i < spatialVelocityCommands.size(); i++) { SpatialVelocityCommand command = spatialVelocityCommands.get(i); RigidBodyBasics base = nameToRigidBodyMap.get(command.getBaseName()); RigidBodyBasics endEffector = nameToRigidBodyMap.get(command.getEndEffectorName()); command.set(base, endEffector); } }