/** * Collects only the joints from {@code source} that are instances of the given {@code clazz} and * stores them in {@code destination}. * <p> * WARNING: This method generates garbage. * </p> * * @param source the original collection of joints to filter. Not modified. * @param clazz the class that the filtered joints have to implement. * @return the filtered joints. */ public static <T extends JointReadOnly> List<T> filterJoints(List<? extends JointReadOnly> source, Class<T> clazz) { List<T> filteredJoints = new ArrayList<>(); filterJoints(source, filteredJoints, clazz); return filteredJoints; }
public RandomRestartInverseKinematicsCalculator(int maxRestarts, double restartTolerance, GeometricJacobian jacobian, NumericalInverseKinematicsCalculator inverseKinematicsCalculator) { this.inverseKinematicsCalculator = inverseKinematicsCalculator; this.joints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); this.maxRestarts = maxRestarts; this.restartTolerance = restartTolerance; }
/** * Travels the multi-body system from {@code start} to {@code end} and stores in order the joints * that implement {@code OneDoFJointBasics} and that are in between and return them as an array. * <p> * WARNING: This method generates garbage. * </p> * * @param start the rigid-body from where to begin the collection of joints. * @param end the rigid-body where to stop the collection of joints. * @return the array of joints representing the path from {@code start} to {@code end}. */ public static OneDoFJointBasics[] createOneDoFJointPath(RigidBodyBasics start, RigidBodyBasics end) { return filterJoints(createJointPath(start, end), OneDoFJointBasics.class); }
/** * Travels the multi-body system from {@code start} to {@code end} and stores in order the joints * that are in between and return them as an array. * <p> * WARNING: This method generates garbage. * </p> * * @param start the rigid-body from where to begin the collection of joints. * @param end the rigid-body where to stop the collection of joints. * @return the array of joints representing the path from {@code start} to {@code end}, or * {@code null} if the given rigid-bodies are not part of the same multi-body system. */ public static JointBasics[] createJointPath(RigidBodyBasics start, RigidBodyBasics end) { return filterJoints(createJointPath((RigidBodyReadOnly) start, (RigidBodyReadOnly) end), JointBasics.class); }
/** * Collects and returns only the joints from {@code source} that are instances of the given * {@code clazz}. * <p> * WARNING: This method generates garbage. * </p> * * @param source the original array of joints to filter. Not modified. * @param clazz the class that the filtered joints have to implement. * @return the array containing the filtered joints. */ public static <T extends JointReadOnly> T[] filterJoints(JointReadOnly[] source, Class<T> clazz) { @SuppressWarnings("unchecked") T[] retArray = (T[]) Array.newInstance(clazz, computeNumberOfJointsOfType(clazz, source)); filterJoints(source, retArray, clazz); return retArray; }
/** * Performs a deep copy of the given {@code originalJoints} then filters the cloned joint to return * only the ones that implement the given {@code clazz}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @param clazz class used to filter the cloned joints that are to be returned. * @return the clone kinematic chain. */ public static <T extends JointReadOnly> T[] cloneKinematicChainAndFilter(T[] originalJoints, Class<T> clazz) { return MultiBodySystemTools.filterJoints(cloneKinematicChain(originalJoints), clazz); }
/** * Performs a deep copy of the given {@code originalJoints} then filters the cloned joint to return * only the ones that implement the given {@code clazz}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @param clazz class used to filter the cloned joints that are to be returned. * @param cloneSuffix suffix to append to the cloned joints and rigid-bodies. * @return the clone kinematic chain. */ public static <T extends JointReadOnly> T[] cloneKinematicChainAndFilter(T[] originalJoints, Class<T> clazz, String cloneSuffix) { return MultiBodySystemTools.filterJoints(cloneKinematicChain(originalJoints, cloneSuffix), clazz); }
private void getAllJointsExcludingHands(ArrayList<OneDoFJointBasics> jointsToPack) { getOneDoFJoints(jointsToPack); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics hand = getHand(robotSide); if (hand != null) { OneDoFJointBasics[] fingerJoints = MultiBodySystemTools.filterJoints(MultiBodySystemTools.collectSubtreeJoints(hand), OneDoFJointBasics.class); for (OneDoFJointBasics fingerJoint : fingerJoints) { jointsToPack.remove(fingerJoint); } } } }
public static void getAllJointsExcludingHands(ArrayList<OneDoFJointBasics> jointsToPack, FullHumanoidRobotModel model) { model.getOneDoFJoints(jointsToPack); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics hand = model.getHand(robotSide); if (hand != null) { OneDoFJointBasics[] fingerJoints = MultiBodySystemTools.filterJoints(MultiBodySystemTools.collectSubtreeJoints(hand), OneDoFJointBasics.class); for (OneDoFJointBasics fingerJoint : fingerJoints) { jointsToPack.remove(fingerJoint); } } } } }
private void checkRobotMatchesData(String robotName, RigidBodyBasics rootBody, HSSFSheet descriptionSheet) { String robotNameInWorkbook = descriptionSheet.getRow(0).getCell(1).getStringCellValue(); if (!robotName.equals(robotNameInWorkbook)) { throw new RuntimeException("Trying to load the data for another robot: Loading data for " + robotName + ", workbook contains data for " + robotNameInWorkbook); } ArrayList<String> jointNames = new ArrayList<>(); int currentIndexValue = 2; HSSFRow currentRow = descriptionSheet.getRow(11); HSSFCell currentCell = currentRow.getCell(currentIndexValue); while (currentCell != null) { jointNames.add(currentCell.getStringCellValue()); currentCell = currentRow.getCell(currentIndexValue++); } JointBasics[] joints = ScrewTools.findJointsWithNames(MultiBodySystemTools.collectSubtreeJoints(rootBody), jointNames.toArray(new String[0])); OneDoFJointBasics[] oneDoFJoints = MultiBodySystemTools.filterJoints(joints, OneDoFJointBasics.class); if (oneDoFJoints.length != jointNames.size()) { throw new RuntimeException("Could not find all the joints"); } }
public ForwardDynamicComparisonScript(Robot robot, DRCRobotModel robotModel) { this.robot = robot; FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); List<JointBasics> ignoredJoints = robotModel.getJointMap().getLastSimulatedJoints().stream() .flatMap(name -> SubtreeStreams.fromChildren(fullRobotModel.getOneDoFJointByName(name).getSuccessor())) .collect(Collectors.toList()); multiBodySystem = MultiBodySystemBasics.toMultiBodySystemBasics(fullRobotModel.getElevator(), ignoredJoints); calculator = new ForwardDynamicsCalculator(multiBodySystem); floatingJoint = fullRobotModel.getRootJoint(); allOneDoFJoints = MultiBodySystemTools.filterJoints(multiBodySystem.getJointsToConsider(), OneDoFJointBasics.class); multiBodySystem.getAllJoints().forEach(joint -> nameToJointMap.put(joint.getName(), joint)); }
this.solveOrientation = solveOrientation; this.oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints");
protected BodyVelocitySensor(String prefix, double dt, RigidBodyBasics body, ReferenceFrame measurementFrame, boolean estimateBias, DoubleProvider variance, YoVariableRegistry registry) { this.sqrtHz = 1.0 / Math.sqrt(dt); this.variance = variance; measurement = new FrameVector3D(measurementFrame); robotJacobian.setKinematicChain(MultiBodySystemTools.getRootBody(body), body); robotJacobian.setJacobianFrame(measurementFrame); List<OneDoFJointBasics> oneDofJoints = MultiBodySystemTools.filterJoints(robotJacobian.getJointsFromBaseToEndEffector(), OneDoFJointBasics.class); oneDofJoints.stream().forEach(joint -> oneDofJointNames.add(joint.getName())); if (estimateBias) { biasState = new BiasState(prefix, dt, registry); } else { biasState = null; } int degreesOfFreedom = robotJacobian.getNumberOfDegreesOfFreedom(); jacobianRelevantPart.reshape(getMeasurementSize(), degreesOfFreedom); }
public NumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double lambdaLeastSquares, double tolerance, int maxIterations, double maxStepSize, double minRandomSearchScalar, double maxRandomSearchScalar) { if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); this.jacobian = jacobian; numberOfConstraints = SpatialVector.SIZE; numberOfDoF = jacobian.getNumberOfColumns(); inverseJacobianCalculator = InverseJacobianSolver.createInverseJacobianSolver(numberOfConstraints, numberOfDoF, false); oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); jointAnglesCorrection = new DenseMatrix64F(numberOfDoF, 1); jointAnglesMinimumError = new DenseMatrix64F(numberOfDoF, 1); this.lambdaLeastSquares = lambdaLeastSquares; this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testFilterJoints() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); RevoluteJoint[] justRevolutes = MultiBodySystemTools.filterJoints(jointsArr, RevoluteJoint.class); assertEquals(jointsArr.length - 1, justRevolutes.length); SixDoFJoint[] justSix = MultiBodySystemTools.filterJoints(jointsArr, SixDoFJoint.class); assertEquals(1, justSix.length); assertTrue(justSix[0] instanceof SixDoFJoint); Boolean clean = false; for(JointBasics joint: justRevolutes) { if(joint instanceof RevoluteJoint) { clean = true; } else { clean = false; } assertTrue(clean); } }
public ReNumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double tolerance, int maxIterations, double maxStepSize, double minRandomSearchScalar, double maxRandomSearchScalar) { if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); this.jacobian = jacobian; this.solver = LinearSolverFactory.leastSquares(SpatialVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = MultiBodySystemTools.computeDegreesOfFreedom(oneDoFJoints); correction = new DenseMatrix64F(nDoF, 1); current = new DenseMatrix64F(nDoF, 1); best = new DenseMatrix64F(nDoF, 1); this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; counter = 0; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testExtractRevoluteJoints() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArr = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); RevoluteJoint[] revoluteJoints = MultiBodySystemTools.filterJoints(jointsArr, RevoluteJoint.class); assertEquals(jointsArr.length - 1, revoluteJoints.length); for(int i = 0; i < revoluteJoints.length; i++) { assertEquals("testJoint" + i, revoluteJoints[i].getName()); } }
public KinematicSolver(GeometricJacobian jacobian, double tolerance, double maxIterations) { this.jacobian = jacobian; this.tolerance = tolerance; this.maxIterations = maxIterations; this.oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); nDoF = MultiBodySystemTools.computeDegreesOfFreedom(oneDoFJoints); jacobianMethod = new DenseMatrix64F(nDoF, nDoF); jacobianTranspose = new DenseMatrix64F(nDoF, nDoF); jacobianJacobianTranspose = new DenseMatrix64F(nDoF, nDoF); jJTe = new DenseMatrix64F(nDoF, 1); correction = new DenseMatrix64F(nDoF, 1); spatialError = new DenseMatrix64F(SpatialVector.SIZE, 1); dampingSquaredDiagonal = new DenseMatrix64F(nDoF, nDoF); inverseTerm = new DenseMatrix64F(nDoF, nDoF); convergeRate = 1E-8; solveMethod = JacobianMethod.JACOBIAN_TRANSPOSE; dampingConstant = 0.3; }
public static void main(String[] args) throws IOException { FramePose3D framePose = new FramePose3D(); framePose.setOrientationYawPitchRoll(1.0, 0.8, -1.1); framePose.setPosition(3.1, 0.1, 1.0); System.out.println(framePose.getOrientation()); RigidBodyTransform transformToParent = new RigidBodyTransform(); framePose.get(transformToParent); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("blop", ReferenceFrame.getWorldFrame(), transformToParent); SphereVoxelShape sphereVoxelShape = new SphereVoxelShape(gridFrame, 0.1, 10, 12, SphereVoxelType.graspOrigin); Voxel3DGrid voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, 10, 0.1); RobotArm robot = new RobotArm(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJointBasics.class); ReachabilityMapFileWriter reachabilityMapFileWriter = new ReachabilityMapFileWriter(robot.getName(), ReachabilityMapFileWriter.class); reachabilityMapFileWriter.initialize(armJoints, voxel3dGrid); for (int i = 0; i < 70000; i++) reachabilityMapFileWriter.registerReachablePose(0, 1, 2, 3, 4); reachabilityMapFileWriter.exportAndClose(); } }
public ReachabilitySphereMapExample() { final RobotArm robot = new RobotArm(); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); Graphics3DObject coordinate = new Graphics3DObject(); coordinate.transform(robot.getElevatorFrameTransformToWorld()); coordinate.addCoordinateSystem(1.0); scs.addStaticLinkGraphics(coordinate); scs.startOnAThread(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJointBasics.class); ReachabilitySphereMapCalculator reachabilitySphereMapCalculator = new ReachabilitySphereMapCalculator(armJoints, scs); // reachabilitySphereMapCalculator.setControlFrameFixedInEndEffector(robot.getControlFrame()); ReachabilityMapListener listener = new ReachabilityMapListener() { @Override public void hasReachedNewConfiguration() { robot.copyRevoluteJointConfigurationToPinJoints(); } }; reachabilitySphereMapCalculator.attachReachabilityMapListener(listener); reachabilitySphereMapCalculator.buildReachabilitySpace(); }