public static <T extends InverseDynamicsJoint> List<T> filterJoints(List<InverseDynamicsJoint> source, Class<T> clazz) { List<T> retList = new ArrayList<>(); filterJoints(source, retList, clazz); return retList; }
public RandomRestartInverseKinematicsCalculator(int maxRestarts, double restartTolerance, GeometricJacobian jacobian, NumericalInverseKinematicsCalculator inverseKinematicsCalculator) { this.inverseKinematicsCalculator = inverseKinematicsCalculator; this.joints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); this.maxRestarts = maxRestarts; this.restartTolerance = restartTolerance; }
public static <T extends InverseDynamicsJoint> T[] filterJoints(InverseDynamicsJoint[] source, Class<T> clazz) { @SuppressWarnings("unchecked") T[] retArray = (T[]) Array.newInstance(clazz, ScrewTools.computeNumberOfJointsOfType(clazz, source)); filterJoints(source, retArray, clazz); return retArray; }
public static OneDoFJoint[] createOneDoFJointPath(RigidBody start, RigidBody end) { return filterJoints(createJointPath(start, end), OneDoFJoint.class); }
public static <T extends InverseDynamicsJoint> T[] cloneJointPathAndFilter(T[] joints, Class<T> clazz) { return filterJoints(cloneJointPath(joints), clazz); }
public static <T extends InverseDynamicsJoint> T[] cloneJointPathAndFilter(T[] joints, Class<T> clazz, String suffix) { return filterJoints(cloneJointPath(joints, suffix), clazz); }
public static <T extends InverseDynamicsJoint> T[] cloneJointPathDisconnectedFromOriginalRobot(T[] joints, Class<T> clazz, String suffix, ReferenceFrame rootBodyFrame) { return filterJoints(cloneJointPathDisconnectedFromOriginalRobot(joints, suffix, rootBodyFrame), clazz); }
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); }
public static List<OneDoFJoint> createRandomTreeRobotWithOneDoFJoints(String prefix, RigidBody rootBody, int numberOfJoints, Random random) { List<OneDoFJoint> tempJoints = new ArrayList<>(); RigidBody predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint joint; if (random.nextBoolean()) joint = addRandomPrismaticJoint(prefix + "Joint" + i, random, predecessor); else joint = addRandomRevoluteJoint(prefix + "Joint" + i, random, predecessor); ScrewTestTools.addRandomRigidBody(prefix + "Body" + i, random, joint); tempJoints.add(joint); predecessor = tempJoints.get(random.nextInt(tempJoints.size())).getSuccessor(); } OneDoFJoint[] jointArray = ScrewTools.filterJoints(ScrewTools.computeSubtreeJoints(rootBody), OneDoFJoint.class); return Arrays.asList(jointArray); }
public JointIndexHandler(InverseDynamicsJoint[] jointsToIndex) { indexedJoints = jointsToIndex; indexedOneDoFJoints = ScrewTools.filterJoints(indexedJoints, OneDoFJoint.class); numberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsToIndex); for (InverseDynamicsJoint joint : jointsToIndex) { TIntArrayList listToPackIndices = new TIntArrayList(); ScrewTools.computeIndexForJoint(jointsToIndex, listToPackIndices, joint); int[] indices = listToPackIndices.toArray(); columnsForJoints.put(joint, indices); } }
private void checkRobotMatchesData(String robotName, RigidBody 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++); } InverseDynamicsJoint[] joints = ScrewTools.findJointsWithNames(ScrewTools.computeSubtreeJoints(rootBody), jointNames.toArray(new String[0])); OneDoFJoint[] oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); if (oneDoFJoints.length != jointNames.size()) { throw new RuntimeException("Could not find all the joints"); } }
public AntiGravityJointTorquesVisualizer(FullRobotModel fullRobotModel, TwistCalculator twistCalculator, SideDependentList<WrenchBasedFootSwitch> wrenchBasedFootSwitches, YoVariableRegistry parentRegistry, double gravity) { SpatialAccelerationVector rootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(twistCalculator.getRootBody(), gravity); this.inverseDynamicsCalculator = new InverseDynamicsCalculator(ReferenceFrame.getWorldFrame(), rootAcceleration, new LinkedHashMap<RigidBody, Wrench>(), new ArrayList<InverseDynamicsJoint>(), false, false, twistCalculator); this.wrenchBasedFootSwitches = wrenchBasedFootSwitches; allJoints = ScrewTools.computeSubtreeJoints(inverseDynamicsCalculator.getSpatialAccelerationCalculator().getRootBody()); allOneDoFJoints = ScrewTools.filterJoints(allJoints, OneDoFJoint.class); antiGravityJointTorques = new LinkedHashMap<>(allOneDoFJoints.length); for (int i = 0; i < allOneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint = allOneDoFJoints[i]; DoubleYoVariable antiGravityJointTorque = new DoubleYoVariable("antiGravity_tau_" + oneDoFJoint.getName(), registry); antiGravityJointTorques.put(oneDoFJoint, antiGravityJointTorque); } parentRegistry.addChild(registry); }
public JointStateFullRobotModelUpdater(ControlFlowGraph controlFlowGraph, Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointPositionSensors, Map<OneDoFJoint, ControlFlowOutputPort<double[]>> jointVelocitySensors, FullInverseDynamicsStructure inverseDynamicsStructure) { InverseDynamicsJoint[] joints = ScrewTools.computeSupportAndSubtreeJoints(inverseDynamicsStructure.getRootJoint().getSuccessor()); this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); this.inverseDynamicsStructureOutputPort = createOutputPort("inverseDynamicsStructureOutputPort"); inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { if (jointPositionSensors.get(oneDoFJoint) == null) { throw new RuntimeException("positionSensorPorts.get(oneDoFJoint) == null. oneDoFJoint = " + oneDoFJoint); } } for (OneDoFJoint oneDoFJoint : oneDoFJoints) { ControlFlowOutputPort<double[]> positionSensorOutputPort = jointPositionSensors.get(oneDoFJoint); ControlFlowInputPort<double[]> positionSensorInputPort = createInputPort("positionSensorInputPort"); positionSensorInputPorts.put(oneDoFJoint, positionSensorInputPort); controlFlowGraph.connectElements(positionSensorOutputPort, positionSensorInputPort); ControlFlowOutputPort<double[]> velocitySensorOutputPort = jointVelocitySensors.get(oneDoFJoint); ControlFlowInputPort<double[]> velocitySensorInputPort = createInputPort("velocitySensorInputPort"); velocitySensorInputPorts.put(oneDoFJoint, velocitySensorInputPort); controlFlowGraph.connectElements(velocitySensorOutputPort, velocitySensorInputPort); } }
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 = SpatialMotionVector.SIZE; numberOfDoF = jacobian.getNumberOfColumns(); inverseJacobianCalculator = InverseJacobianSolver.createInverseJacobianSolver(numberOfConstraints, numberOfDoF, false); oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.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; }
public JointStateUpdater(FullInverseDynamicsStructure inverseDynamicsStructure, SensorOutputMapReadOnly sensorOutputMapReadOnly, StateEstimatorParameters stateEstimatorParameters, YoVariableRegistry parentRegistry) { twistCalculator = inverseDynamicsStructure.getTwistCalculator(); spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); rootBody = twistCalculator.getRootBody(); this.sensorMap = sensorOutputMapReadOnly; InverseDynamicsJoint[] joints = ScrewTools.computeSupportAndSubtreeJoints(inverseDynamicsStructure.getRootJoint().getSuccessor()); this.oneDoFJoints = ScrewTools.filterJoints(joints, OneDoFJoint.class); iMUBasedJointVelocityEstimator = createIMUBasedJointVelocityEstimator(sensorOutputMapReadOnly, stateEstimatorParameters, registry); parentRegistry.addChild(registry); }
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(SpatialMotionVector.SIZE, jacobian.getNumberOfColumns()); // new DampedLeastSquaresSolver(jacobian.getNumberOfColumns()); this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); oneDoFJointsSeed = oneDoFJoints.clone(); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); int nDoF = ScrewTools.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; }
public IMUBasedJointVelocityEstimator(IMUSensorReadOnly pelvisIMU, IMUSensorReadOnly chestIMU, SensorOutputMapReadOnly sensorMap, double estimatorDT, double slopTime, YoVariableRegistry registry) { this.sensorMap = sensorMap; this.pelvisIMU = pelvisIMU; this.chestIMU = chestIMU; jacobian = new GeometricJacobian(pelvisIMU.getMeasurementLink(), chestIMU.getMeasurementLink(), chestIMU.getMeasurementLink().getBodyFixedFrame()); joints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); String namePrefix = "imuBasedJointVelocityEstimator"; alpha = new DoubleYoVariable(namePrefix + "AlphaFuse", registry); alpha.set(0.0); double dt = estimatorDT; this.slopTime = new DoubleYoVariable(namePrefix + "SlopTime", registry); this.slopTime.set(slopTime); for (OneDoFJoint joint : joints) { jointVelocitiesFromIMUOnly.put(joint, new DoubleYoVariable("qd_" + joint.getName() + "_IMUBased", registry)); jointVelocities.put(joint, new BacklashProcessingYoVariable("qd_" + joint.getName() + "_FusedWithIMU", "", dt, this.slopTime, registry)); } }
public KinematicSolver(GeometricJacobian jacobian, double tolerance, double maxIterations) { this.jacobian = jacobian; this.tolerance = tolerance; this.maxIterations = maxIterations; this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); nDoF = ScrewTools.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(SpatialMotionVector.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 { FramePose framePose = new FramePose(); framePose.setYawPitchRoll(1.0, 0.8, -1.1); framePose.setPosition(3.1, 0.1, 1.0); System.out.println(framePose.getFrameOrientationCopy().toStringAsQuaternion()); RigidBodyTransform transformToParent = new RigidBodyTransform(); framePose.getPose(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(); OneDoFJoint[] armJoints = ScrewTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJoint.class); ReachabilityMapFileWriter reachabilityMapFileWriter = new ReachabilityMapFileWriter(robot.getName(), armJoints, voxel3dGrid, ReachabilityMapFileWriter.class); 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(); OneDoFJoint[] armJoints = ScrewTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJoint.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(); }