/** * Creates a {@code PrivilegedConfigurationCommand} to update the privileged joint angles to * match the current state of {@link #desiredFullRobotModel}. */ private void snapPrivilegedConfigurationToCurrent() { PrivilegedConfigurationCommand privilegedConfigurationCommand = new PrivilegedConfigurationCommand(); privilegedConfigurationCommand.setPrivilegedConfigurationOption(PrivilegedConfigurationOption.AT_CURRENT); privilegedConfigurationCommand.setDefaultWeight(privilegedWeight.getDoubleValue()); privilegedConfigurationCommand.setDefaultConfigurationGain(privilegedConfigurationGain.getDoubleValue()); privilegedConfigurationCommand.setDefaultMaxVelocity(privilegedMaxVelocity.getDoubleValue()); privilegedConfigurationCommandReference.set(privilegedConfigurationCommand); }
private void updatePrivilegedConfiguration() { double timeInTrajectory = MathTools.clipToMinMax(getTimeInCurrentState(), 0.0, durationForStanceLegStraightening.getDoubleValue()); kneePrivilegedConfigurationTrajectory.compute(timeInTrajectory); straightLegsPrivilegedConfigurationCommand.clear(); straightLegsPrivilegedConfigurationCommand.addJoint(kneePitch, kneePrivilegedConfigurationTrajectory.getPosition()); straightLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, contactableFoot.getRigidBody()); }
public AbstractFootControlState(ConstraintType stateEnum, FootControlHelper footControlHelper) { super(stateEnum); this.footControlHelper = footControlHelper; this.contactableFoot = footControlHelper.getContactableFoot(); this.momentumBasedController = footControlHelper.getMomentumBasedController(); this.robotSide = footControlHelper.getRobotSide(); rootBody = momentumBasedController.getTwistCalculator().getRootBody(); FullHumanoidRobotModel fullRobotModel = footControlHelper.getMomentumBasedController().getFullRobotModel(); RigidBody pelvis = fullRobotModel.getPelvis(); RigidBody foot = fullRobotModel.getFoot(robotSide); OneDoFJoint kneeJoint = fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH); straightLegsPrivilegedConfigurationCommand.addJoint(kneeJoint, PrivilegedConfigurationOption.AT_ZERO); straightLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, foot); bentLegsPrivilegedConfigurationCommand.addJoint(kneeJoint, PrivilegedConfigurationOption.AT_MID_RANGE); bentLegsPrivilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(pelvis, foot); }
PrivilegedConfigurationCommand privilegedConfigurationCommand = new PrivilegedConfigurationCommand(); privilegedConfigurationCommand.setPrivilegedConfigurationOption(PrivilegedConfigurationOption.AT_CURRENT); privilegedConfigurationCommand.setDefaultWeight(privilegedWeight.getDoubleValue()); privilegedConfigurationCommand.setConfigurationGain(privilegedConfigurationGain.getDoubleValue()); privilegedConfigurationCommand.setMaxVelocity(privilegedMaxVelocity.getDoubleValue()); privilegedConfigurationCommandReference.set(privilegedConfigurationCommand);
privilegedConfigurationCommand.clear(); privilegedConfigurationCommand.setPrivilegedConfigurationOption(PrivilegedConfigurationOption.AT_ZERO); privilegedConfigurationCommand.addJoint(fullRobotModel.getArmJoint(robotSide, armJointNames[i]), PrivilegedConfigurationOption.AT_MID_RANGE);
privilegedConfigurationCommand.applyPrivilegedConfigurationToSubChain(chest, endEffector);
PrivilegedConfigurationCommand privilegedConfigurationCommand = new PrivilegedConfigurationCommand(); privilegedConfigurationCommand.setPrivilegedConfigurationOption(PrivilegedConfigurationOption.AT_CURRENT); privilegedConfigurationCommand.setDefaultWeight(privilegedWeight.getDoubleValue()); privilegedConfigurationCommand.setConfigurationGain(privilegedConfigurationGain.getDoubleValue()); privilegedConfigurationCommand.setMaxVelocity(privilegedMaxVelocity.getDoubleValue()); privilegedConfigurationCommandReference.set(privilegedConfigurationCommand);