Twist desiredHandTwist = computeDesiredTwist(desiredHandPose, hand, handControlFrame, selectionMatrix, tempErrorMagnitude);
newSolutionQuality += tempErrorMagnitude.doubleValue();
SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand();
spatialVelocityCommand.set(elevator, hand);
spatialVelocityCommand.set(desiredHandTwist, selectionMatrix);
Twist desiredFootTwist = computeDesiredTwist(desiredFootPose, foot, selectionMatrix, tempErrorMagnitude);
newSolutionQuality += tempErrorMagnitude.doubleValue();
SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand();
spatialVelocityCommand.set(elevator, foot);
spatialVelocityCommand.set(desiredFootTwist, selectionMatrix);
ReferenceFrame chestFrame = chest.getBodyFixedFrame();
FrameVector desiredChestAngularVelocity = computeDesiredAngularVelocity(desiredChestOrientation, chestFrame);
SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand();
spatialVelocityCommand.set(elevator, chest);
spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity);
ReferenceFrame pelvisFrame = pelvis.getBodyFixedFrame();
FrameVector desiredPelvisAngularVelocity = computeDesiredAngularVelocity(desiredPelvisOrientation, pelvisFrame);
SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand();
spatialVelocityCommand.set(elevator, pelvis);
spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity);