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); } }
public void removeWeight() { setWeight(HARD_CONSTRAINT); }
public void retrieveRigidBodiesFromName(Map<String, RigidBody> nameToRigidBodyMap) { for (int i = 0; i < spatialVelocityCommands.size(); i++) { SpatialVelocityCommand command = spatialVelocityCommands.get(i); command.setBase(nameToRigidBodyMap.get(command.getBaseName())); command.setEndEffector(nameToRigidBodyMap.get(command.getEndEffectorName())); } }
public void set(RigidBody base, RigidBody endEffector) { setBase(base); setEndEffector(endEffector); }
Twist desiredHandTwist = computeDesiredTwist(desiredHandPose, hand, handControlFrame, selectionMatrix, tempErrorMagnitude); newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, hand); spatialVelocityCommand.set(desiredHandTwist, selectionMatrix); spatialVelocityCommand.setWeight(handWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); Twist desiredFootTwist = computeDesiredTwist(desiredFootPose, foot, selectionMatrix, tempErrorMagnitude); newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, foot); spatialVelocityCommand.set(desiredFootTwist, selectionMatrix); spatialVelocityCommand.setWeight(footWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); FrameVector desiredChestAngularVelocity = new FrameVector(); 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); FrameVector desiredPelvisAngularVelocity = new FrameVector(); desiredPelvisTwist.getAngularVelocityInBaseFrame(desiredPelvisAngularVelocity); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, pelvis);
private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy) { SpatialVelocityCommand localCommand = spatialVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }
Twist desiredHandTwist = computeDesiredTwist(desiredHandPose, hand, handControlFrame, selectionMatrix, tempErrorMagnitude); newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, hand); spatialVelocityCommand.set(desiredHandTwist, selectionMatrix); spatialVelocityCommand.setWeight(handWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); Twist desiredFootTwist = computeDesiredTwist(desiredFootPose, foot, selectionMatrix, tempErrorMagnitude); newSolutionQuality += tempErrorMagnitude.doubleValue(); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, foot); spatialVelocityCommand.set(desiredFootTwist, selectionMatrix); spatialVelocityCommand.setWeight(footWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); ReferenceFrame chestFrame = chest.getBodyFixedFrame(); FrameVector desiredChestAngularVelocity = computeDesiredAngularVelocity(desiredChestOrientation, chestFrame); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, chest); spatialVelocityCommand.setAngularVelocity(chestFrame, elevatorFrame, desiredChestAngularVelocity); spatialVelocityCommand.setWeight(chestWeight.getDoubleValue()); ret.addCommand(spatialVelocityCommand); ReferenceFrame pelvisFrame = pelvis.getBodyFixedFrame(); FrameVector desiredPelvisAngularVelocity = computeDesiredAngularVelocity(desiredPelvisOrientation, pelvisFrame); SpatialVelocityCommand spatialVelocityCommand = new SpatialVelocityCommand(); spatialVelocityCommand.set(elevator, pelvis); spatialVelocityCommand.setAngularVelocity(pelvisFrame, elevatorFrame, desiredPelvisAngularVelocity);
private void copySpatialVelocityCommand(SpatialVelocityCommand commandToCopy) { SpatialVelocityCommand localCommand = spatialVelocityCommands.add(); localCommand.set(commandToCopy); inverseKinematicsCommandList.addCommand(localCommand); }