/** * Creates and sets up the {@code JointLimitReductionCommand} from the map * {@link #legJointLimitReductionFactors}. * * @return the command for reducing the allowed range of motion of the leg joints. */ private JointLimitReductionCommand createJointLimitReductionCommand() { JointLimitReductionCommand jointLimitReductionCommand = new JointLimitReductionCommand(); for (RobotSide robotSide : RobotSide.values) { for (LegJointName legJointName : desiredFullRobotModel.getRobotSpecificJointNames().getLegJointNames()) { OneDoFJointBasics joint = desiredFullRobotModel.getLegJoint(robotSide, legJointName); double reductionFactor = legJointLimitReductionFactors.get(legJointName).getDoubleValue(); jointLimitReductionCommand.addReductionFactor(joint, reductionFactor); } } return jointLimitReductionCommand; }
@Override public void set(JointLimitReductionCommand other) { clear(); for (int i = 0; i < other.getNumberOfJoints(); i++) { jointNames.add(other.jointNames.get(i)); joints.add(other.joints.get(i)); jointReductionFactors.add(other.jointReductionFactors.get(i)); } }
public void submitJointLimitReductionCommand(JointLimitReductionCommand command) { for (int commandJointIndex = 0; commandJointIndex < command.getNumberOfJoints(); commandJointIndex++) { OneDoFJoint joint = command.getJoint(commandJointIndex); int jointIndex = jointIndexHandler.getOneDoFJointIndex(joint); double originalJointLimitLower = joint.getJointLimitLower(); double originalJointLimitUpper = joint.getJointLimitUpper(); double limitReduction = command.getJointLimitReductionFactor(commandJointIndex) * jointsRangeOfMotion.get(jointIndex, 0); jointLowerLimits.set(jointIndex, 0, originalJointLimitLower + limitReduction); jointUpperLimits.set(jointIndex, 0, originalJointLimitUpper - limitReduction); } }
JointLimitReductionCommand jointLimitReductionCommand = new JointLimitReductionCommand(); for (RobotSide robotSide : RobotSide.values) jointLimitReductionCommand.addReductionFactor(joint, reductionFactor);
JointLimitReductionCommand jointLimitReductionCommand = new JointLimitReductionCommand(); for (RobotSide robotSide : RobotSide.values) jointLimitReductionCommand.addReductionFactor(joint, reductionFactor);