public static void createRandomChainRobotWithOneDoFJoints(String prefix, List<OneDoFJoint> oneDoFJointsToPack, RigidBody rootBody, Vector3d[] jointAxes, Random random) { RigidBody predecessor = rootBody; for (int i = 0; i < jointAxes.length; i++) { OneDoFJoint joint; if (random.nextBoolean()) joint = addRandomPrismaticJoint(prefix + "Joint" + i, jointAxes[i], random, predecessor); else joint = addRandomRevoluteJoint(prefix + "Joint" + i, jointAxes[i], random, predecessor); oneDoFJointsToPack.add(joint); predecessor = ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); } }
public static List<RevoluteJoint> createRandomChainRobot(int numberOfJoints, Random random) { return createRandomChainRobot("", numberOfJoints, random); }
public static List<OneDoFJoint> createRandomChainRobotWithOneDoFJoints(int numberOfJoints, Random random) { return createRandomChainRobotWithOneDoFJoints("", numberOfJoints, random); }
public static void createRandomChainRobotWithPrismaticJoints(String prefix, List<PrismaticJoint> prismaticJointsToPack, RigidBody rootBody, Vector3d[] jointAxes, Random random) { RigidBody predecessor = rootBody; for (int i = 0; i < jointAxes.length; i++) { PrismaticJoint joint = addRandomPrismaticJoint(prefix + "Joint" + i, jointAxes[i], random, predecessor); prismaticJointsToPack.add(joint); predecessor = ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); } }
public static void createRandomChainRobot(String prefix, List<RevoluteJoint> revoluteJointsToPack, RigidBody rootBody, Vector3d[] jointAxes, Random random) { RigidBody currentIDBody = rootBody; for (int i = 0; i < jointAxes.length; i++) { RevoluteJoint currentIDJoint = addRandomRevoluteJoint(prefix + "joint" + i, jointAxes[i], random, currentIDBody); currentIDBody = addRandomRigidBody(prefix + "body" + i, random, currentIDJoint); revoluteJointsToPack.add(currentIDJoint); } rootBody.updateFramesRecursively(); }
public RandomFloatingChain(Random random, Vector3d[] jointAxes) { ReferenceFrame elevatorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("elevator", ReferenceFrame.getWorldFrame(), new RigidBodyTransform()); elevator = new RigidBody("elevator", elevatorFrame); rootJoint = new SixDoFJoint("rootJoint", elevator, elevatorFrame); RigidBody rootBody = ScrewTestTools.addRandomRigidBody("rootBody", random, rootJoint); revoluteJoints = new ArrayList<RevoluteJoint>(); ScrewTestTools.createRandomChainRobot("test", revoluteJoints, rootBody, jointAxes, random); inverseDynamicsJoints.add(rootJoint); inverseDynamicsJoints.addAll(revoluteJoints); }
private boolean isPositionReachable(int numberOfTrials, int xIndex, int yIndex, int zIndex) { boolean success = false; int counter = 0; ScrewTestTools.setRandomPositionsWithinJointLimits(robotArmJoints, random); updateRobotFrames(); voxel3dGrid.getVoxel(voxelLocation, xIndex, yIndex, zIndex); modifiableVoxelLocation.setIncludingFrame(voxelLocation); modifiableVoxelLocation.changeFrame(jacobian.getBaseFrame()); desiredEndEffectorPose.setTranslation(modifiableVoxelLocation.getX(), modifiableVoxelLocation.getY(), modifiableVoxelLocation.getZ()); while (counter < numberOfTrials) { success = linearInverseKinematicsCalculator.solve(desiredEndEffectorPose); if (success) break; ScrewTestTools.setRandomPositionsWithinJointLimits(robotArmJoints, random); updateRobotFrames(); counter++; } return success; } }
sixDoFJoints.set(robotSide, sixDoFJoint); RigidBody rigidBody = ScrewTestTools.addRandomRigidBody(robotSide.getCamelCaseNameForStartOfExpression() + "Foot", random, sixDoFJoint); feet.put(robotSide, rigidBody); double forward = 0.12;
public static List<PrismaticJoint> createRandomChainRobotWithPrismaticJoints(int numberOfJoints, Random random) { return createRandomChainRobotWithPrismaticJoints("", numberOfJoints, random); }
public static List<RevoluteJoint> createRandomTreeRobot(RigidBody rootBody, int numberOfJoints, Random random) { List<RevoluteJoint> joints = new ArrayList<>(); createRandomTreeRobot(joints, rootBody, numberOfJoints, random); return joints; }
public static List<PrismaticJoint> createRandomTreeRobotWithPrismaticJoints(int numberOfJoints, Random random) { return createRandomTreeRobotWithPrismaticJoints("", numberOfJoints, random); }
public static List<OneDoFJoint> createRandomTreeRobotWithOneDoFJoints(int numberOfJoints, Random random) { return createRandomTreeRobotWithOneDoFJoints("", numberOfJoints, random); }
public static PrismaticJoint addRandomPrismaticJoint(String name, Random random, RigidBody predecessor) { Vector3d jointAxis = new Vector3d(random.nextDouble(), random.nextDouble(), random.nextDouble()); return addRandomPrismaticJoint(name, jointAxis, random, predecessor); }
public static RevoluteJoint addRandomRevoluteJoint(String name, Random random, RigidBody predecessor) { Vector3d jointAxis = new Vector3d(random.nextDouble(), random.nextDouble(), random.nextDouble()); return addRandomRevoluteJoint(name, jointAxis, random, predecessor); }
RevoluteJoint currentJoint = addRandomRevoluteJoint("jointID" + i, random, inverseDynamicsParentBody); addRandomRigidBody("bodyID" + i, random, currentJoint);
public static List<PrismaticJoint> createRandomTreeRobotWithPrismaticJoints(String prefix, RigidBody rootBody, int numberOfJoints, Random random) { List<PrismaticJoint> tempJoints = new ArrayList<>(); RigidBody predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { PrismaticJoint joint = addRandomPrismaticJoint(prefix + "Joint" + i, random, predecessor); ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); tempJoints.add(joint); predecessor = tempJoints.get(random.nextInt(tempJoints.size())).getSuccessor(); } PrismaticJoint[] jointArray = ScrewTools.filterJoints(ScrewTools.computeSubtreeJoints(rootBody), PrismaticJoint.class); return Arrays.asList(jointArray); }
ScrewTestTools.setRandomPositionsWithinJointLimits(upperArmJointsClone.get(robotSide), random); return computeUpperArmJointAngles(robotSide, temporaryDesiredUpperArmOrientation, false, iteration + 1);
public static List<PrismaticJoint> createRandomChainRobotWithPrismaticJoints(String prefix, RigidBody rootBody, Vector3d[] jointAxes, Random random) { List<PrismaticJoint> prismaticJoints = new ArrayList<>(); createRandomChainRobotWithPrismaticJoints(prefix, prismaticJoints, rootBody, jointAxes, random); return prismaticJoints; }
public static List<RevoluteJoint> createRandomTreeRobot(int numberOfJoints, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody("RootBody", rootBodyFrame); return createRandomTreeRobot(rootBody, numberOfJoints, random); }
public static List<PrismaticJoint> createRandomTreeRobotWithPrismaticJoints(String prefix, int numberOfJoints, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomTreeRobotWithPrismaticJoints(prefix, rootBody, numberOfJoints, random); }