/** * Generates a random state and update the given {@code joints} with it. * <p> * The random state is guaranteed to be within the joint limits. For instance, a random * configuration is constrained to be in: [{@code joint.getJointLimitLower()}, * {@code joint.getJointLimitUpper()}]. * </p> * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. As no limits are imposed on the * joint accelerations, the state to randomize cannot be the acceleration. For generating * random acceleration, please see * {@link #nextState(Random, JointStateType, double, double, OneDoFJointBasics)}. * @param joints the joints to set the state of. Modified. */ public static void nextStateWithinJointLimits(Random random, JointStateType stateToRandomize, OneDoFJointBasics[] joints) { for (OneDoFJointBasics joint : joints) nextStateWithinJointLimits(random, stateToRandomize, joint); }
/** * Generates a random state and update the given {@code joints} with it. * <p> * The random state is guaranteed to be within the joint limits. For instance, a random * configuration is constrained to be in: [{@code joint.getJointLimitLower()}, * {@code joint.getJointLimitUpper()}]. * </p> * * @param random the random generator to use. * @param stateToRandomize the joint state that is to be randomized. As no limits are imposed on the * joint accelerations, the state to randomize cannot be the acceleration. For generating * random acceleration, please see * {@link #nextState(Random, JointStateType, double, double, OneDoFJointBasics)}. * @param joints the joints to set the state of. Modified. */ public static void nextStateWithinJointLimits(Random random, JointStateType stateToRandomize, Iterable<? extends OneDoFJointBasics> joints) { joints.forEach(joint -> nextStateWithinJointLimits(random, stateToRandomize, joint)); }
private boolean solveAndRetry(int maximumNumberOfIterations) { int tryNumber = 0; boolean success = false; while (!success && tryNumber++ < numberOfTrials) { MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, robotArmJoints); success = solveOnce(maximumNumberOfIterations); } return success; }
private ChestTrajectoryMessage createRandomChestMessage(double trajectoryTime, Random random) { OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); desiredRandomChestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); Quaternion desiredOrientation = new Quaternion(desiredRandomChestOrientation); return HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredOrientation, ReferenceFrame.getWorldFrame(), ReferenceFrame.getWorldFrame()); }
MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame());
OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone);
MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, upperArmJointsClone.get(robotSide)); return computeUpperArmJointAngles(robotSide, temporaryDesiredUpperArmOrientation, false, iteration + 1);
MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone);
MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame());
double trajectoryTime = 1.0; MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, neckJoints); RigidBodyBasics headClone = neckJoints[numberOfJoints - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(headClone.getBodyFixedFrame());