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 Momentum computeCoMMomentum(RigidBodyBasics elevator, ReferenceFrame centerOfMassFrame, CentroidalMomentumCalculator centroidalMomentumMatrix) { DenseMatrix64F mat = centroidalMomentumMatrix.getCentroidalMomentumMatrix(); JointBasics[] jointList = MultiBodySystemTools.collectSubtreeJoints(elevator); DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointList), 1); MultiBodySystemTools.extractJointsState(jointList, JointStateType.VELOCITY, jointVelocities); DenseMatrix64F comMomentumMatrix = MatrixTools.mult(mat, jointVelocities); Momentum comMomentum = new Momentum(centerOfMassFrame, comMomentumMatrix); return comMomentum; } }
/** * 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); }
public static RigidBodyBasics[] computeSupportAndSubtreeSuccessors(RigidBodyBasics... bodies) { return MultiBodySystemTools.collectSuccessors(MultiBodySystemTools.collectSupportAndSubtreeJoints(bodies)); }
public static RigidBodyBasics[] computeSubtreeSuccessors(RigidBodyBasics... bodies) { return MultiBodySystemTools.collectSuccessors(MultiBodySystemTools.collectSubtreeJoints(bodies)); }
/** * Collects and returns all the joints located between the given {@code rigidBody} and the root * body. * <p> * WARNING: This method generates garbage. * </p> * * @param rigidBody the rigid-body to collect the support joints of. * @return the array containing the support joints of the given rigid-body. */ public static JointReadOnly[] collectSupportJoints(RigidBodyReadOnly rigidBody) { return createJointPath(getRootBody(rigidBody), rigidBody); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPackDesiredJointAccelerationsMatrix() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F originalAccel = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < originalAccel.getNumRows() * originalAccel.getNumCols(); i++) { //create original matrix originalAccel.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.ACCELERATION, originalAccel); //set velocities from matrix DenseMatrix64F newAccelerations = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.ACCELERATION, newAccelerations);//pack new matrix for(int i = 0; i < jointsArray.length; i++) { assertEquals("Should be equal velocities", originalAccel.get(i), newAccelerations.get(i), epsilon); } }
JointBasics[] jointsToOptimizeFor = MultiBodySystemTools.collectSupportAndSubtreeJoints(elevator); double controlDT = 0.004; pointFeedbackController.setEnabled(true); int numberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsToOptimizeFor); QPInput motionQPInput = new QPInput(numberOfDoFs); LinearSolver<DenseMatrix64F> pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.VELOCITY, integrator.getJointVelocities()); MultiBodySystemTools.insertJointsState(jointsToOptimizeFor, JointStateType.CONFIGURATION, integrator.getJointConfigurations()); elevator.updateFramesRecursively();
/** * 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 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; }
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); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetVelocities() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < jointVelocities.getNumRows() * jointVelocities.getNumCols(); i++) { jointVelocities.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, jointVelocities); DenseMatrix64F sixDoFVeloc = new DenseMatrix64F(6, 1); jointsArray[0].getJointVelocity(0, sixDoFVeloc); for(int i = 0; i < 6; i++) { assertEquals("Should be equal velocitiess", jointVelocities.get(i), sixDoFVeloc.get(i), epsilon); } OneDoFJointBasics joint; for(int i = 6; i < jointVelocities.getNumRows() * jointVelocities.getNumCols(); i++) { joint = (OneDoFJointBasics)jointsArray[i - 5]; //1 - 6 assertEquals("Should be equal velocities", jointVelocities.get(i), joint.getQd(), epsilon); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeDegreesOfFreedom_Iterable() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); ArrayList<JointBasics> jointsList = new ArrayList<JointBasics>(jointsArray.length); RigidBodyBasics[] partialBodiesArray = ScrewTools.computeSubtreeSuccessors(chain.getElevator()); RigidBodyBasics[] bodiesArray = new RigidBodyBasics[partialBodiesArray.length + 1]; bodiesArray[0] = chain.getElevator(); for(int i = 0; i < partialBodiesArray.length; i++) { bodiesArray[i+1] = partialBodiesArray[i]; } MultiBodySystemTools.computeDegreesOfFreedom(jointsList); }
public static double kinematicsChainLimitScore(RigidBodyBasics start, RigidBodyBasics end) { return jointsLimitScore(MultiBodySystemTools.createOneDoFJointPath(start, end)); }
public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, ArrayList<JointBasics> jointsToIgnore, double gravity) { this(body,ScrewTools.createGravitationalSpatialAcceleration(MultiBodySystemTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); }
/** * 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; }
allJoints = MultiBodySystemTools.collectSupportAndSubtreeJoints(fullRobotModel.getRootJoint().getSuccessor()); v = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(allJoints), 1); JointBasics[] jointPath = MultiBodySystemTools.createJointPath(fullRobotModel.getPelvis(), fullRobotModel.getFoot(robotSide)); actuatedJoints.addAll(Arrays.asList(jointPath)); int nActuatedDoFs = MultiBodySystemTools.computeDegreesOfFreedom(actuatedJoints); vActuated = new DenseMatrix64F(nActuatedDoFs, 1);
/** * 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, RigidBodyBasics body) { this.inertialFrame = inertialFrame; this.rootBody = MultiBodySystemTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = MultiBodySystemTools.collectSubtreeSuccessors(MultiBodySystemTools.collectSubtreeJoints(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)); }
centroidalMomentumMatrix.reset(); DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getCentroidalMomentumMatrix(); DenseMatrix64F jointVelocitiesMatrix = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.VELOCITY, jointVelocitiesMatrix); DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1); CommonOps.mult(centroidalMomentumMatrixMatrix, jointVelocitiesMatrix, momentumFromCentroidalMomentumMatrix);
RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getSuccessor()); MultiBodySystemTools.extractJointsState(rootJacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix); MultiBodySystemTools.extractJointsState(jacobian.getJointsInOrder(), JointStateType.VELOCITY, jointVelocitiesMatrix);