public AbstractInverseDynamicsChecksum(RigidBody rootJoint, GenericCRC32 checksum) { this.checksum = checksum; this.joints = ScrewTools.computeSubtreeJoints(rootJoint); }
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 InverseDynamicsJoint[] computeJointsToOptimizeFor(FullHumanoidRobotModel fullRobotModel, InverseDynamicsJoint... jointsToRemove) { List<InverseDynamicsJoint> joints = new ArrayList<InverseDynamicsJoint>(); InverseDynamicsJoint[] allJoints = ScrewTools.computeSupportAndSubtreeJoints(fullRobotModel.getRootJoint().getSuccessor()); joints.addAll(Arrays.asList(allJoints)); for (RobotSide robotSide : RobotSide.values) { RigidBody hand = fullRobotModel.getHand(robotSide); if (hand != null) { List<InverseDynamicsJoint> fingerJoints = Arrays.asList(ScrewTools.computeSubtreeJoints(hand)); joints.removeAll(fingerJoints); } } if (jointsToRemove != null) { for (InverseDynamicsJoint joint : jointsToRemove) { joints.remove(joint); } } return joints.toArray(new InverseDynamicsJoint[joints.size()]); }
public static RigidBody[] computeSupportAndSubtreeSuccessors(RigidBody... bodies) { return computeSuccessors(computeSupportAndSubtreeJoints(bodies)); }
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); }
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); }
jointNames.toArray(jointNamesArray); InverseDynamicsJoint[] inverseDynamicsJoints = ScrewTools.findJointsWithNames(allJoints, jointNamesArray); OneDoFJoint[] oneDoFJoints = ScrewTools.filterJoints(inverseDynamicsJoints, OneDoFJoint.class); double[] jointAngles = new double[oneDoFJoints.length];
OneDoFJoint[] armJoints = ScrewTools.filterJoints(ScrewTools.createJointPath(chest, hand), OneDoFJoint.class); OneDoFJoint elbowJoint = armJoints[3]; double jointSign = -Math.signum(elbowJoint.getJointLimitLower() + elbowJoint.getJointLimitUpper()); armZeroJointAngleConfigurationOffsets.put(robotSide, armZeroJointAngleConfigurationOffset); upperArmJoints.put(robotSide, ScrewTools.filterJoints(ScrewTools.createJointPath(chest, upperArmBody), OneDoFJoint.class)); upperArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(upperArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); inverseKinematicsForUpperArms.put(robotSide, inverseKinematicsForUpperArm); lowerArmJoints.put(robotSide, ScrewTools.filterJoints(ScrewTools.createJointPath(lowerArmBody, hand), OneDoFJoint.class)); lowerArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(lowerArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide), lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame());
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 PositionStateRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionOutputPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityOutputPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionOutputPort = centerOfMassPositionOutputPort; this.centerOfMassVelocityOutputPort = centerOfMassVelocityOutputPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); }
/** * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies * of the system to which {@code body} belongs. * * @param inertialFrame non-moving frame with respect to which the twists are computed. * Typically {@link ReferenceFrame#getWorldFrame()} is used here. * @param body a body that belongs to the system this twist calculator will be available for. */ public TwistCalculator(ReferenceFrame inertialFrame, RigidBody body) { this.rootBody = ScrewTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = ScrewTools.computeSubtreeSuccessors(ScrewTools.computeSubtreeJoints(rootBody)).length; while (unnassignedTwists.size() < numberOfRigidBodies) unnassignedTwists.add(new Twist()); assignedTwists = new ArrayList<>(numberOfRigidBodies); rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies); assignedTwists.add(rootTwist); rigidBodiesWithAssignedTwist.add(rootBody); rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0)); }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
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); } }
handControlFrame = fullRobotModel.getHandControlFrame(robotSide); controlledJoints = ScrewTools.createOneDoFJointPath(chest, hand); positionControlledJoints = ScrewTools.filterJoints(ScrewTools.findJointsWithNames(controlledJoints, positionControlledJointNames), OneDoFJoint.class); else positionControlledJoints = new OneDoFJoint[0];
public FootSpoof(String name, double xToAnkle, double yToAnkle, double zToAnkle, List<Point2d> contactPoints2dInSoleFrame, double coefficientOfFriction) { RigidBodyTransform transformToAnkle = new RigidBodyTransform(); transformToAnkle.setTranslation(new Vector3d(-xToAnkle, -yToAnkle, -zToAnkle)); // if(FootstepUtilsTest.DEBUG_TESTS) // System.out.println("FootSpoof: making transform from plane to ankle equal to "+transformToAnkle); shinFrame = new PoseReferenceFrame(name + "ShinFrame", ReferenceFrame.getWorldFrame()); this.shin = new RigidBody(name, shinFrame); this.ankle = ScrewTools.addRevoluteJoint(name + "Ankle", shin, new RigidBodyTransform(), new Vector3d(0.0, 1.0, 0.0)); this.foot = ScrewTools.addRigidBody(name, ankle, new Matrix3d(), 1.0, new RigidBodyTransform()); soleFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name + "soleFrame", ankle.getFrameAfterJoint(), transformToAnkle); for (Point2d contactPointInSoleFrame : contactPoints2dInSoleFrame) { FramePoint point = new FramePoint(soleFrame, contactPointInSoleFrame.getX(), contactPointInSoleFrame.getY(), 0.0); contactPoints.add(point); contactPoints2d.add(point.toFramePoint2d()); } totalNumberOfContactPoints = contactPoints.size(); this.coefficientOfFriction = coefficientOfFriction; }
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 static RigidBody[] computeSubtreeSuccessors(RigidBody... bodies) { return computeSuccessors(computeSubtreeJoints(bodies)); }
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); } }