jointsFromBaseToEndEffector.add(null); numberOfDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(base, endEffector);
public OriginalDynamicallyConsistentNullspaceCalculator(FloatingJointBasics rootJoint, boolean computeSNsBar) { this.rootJoint = rootJoint; this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); jointsInOrder = massMatrixCalculator.getInput().getJointMatrixIndexProvider().getIndexedJointsInOrder().toArray(new JointBasics[0]); this.nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); massMatrixSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); lambdaSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); this.computeSNsBar = computeSNsBar; }
/** * Creates the Jacobian for the kinematic chain described by the given joints. These joints have * to be ordered and going downstream (the first joint has to be the closest joint to the root of * the system and the last the farthest). * * @param joints the joints that this Jacobian considers. * @param jacobianFrame the frame in which the resulting twist of the end effector with respect * to the base frame will be expressed. * @param allowChangeFrame whether the feature {@link #changeFrame(ReferenceFrame)} will be * available or not. * @throws RuntimeException if the joint ordering is incorrect. */ public GeometricJacobian(JointBasics[] joints, ReferenceFrame jacobianFrame, boolean allowChangeFrame) { checkJointOrder(joints); this.joints = new JointBasics[joints.length]; System.arraycopy(joints, 0, this.joints, 0, joints.length); this.jacobianFrame = jacobianFrame; this.jacobian = new DenseMatrix64F(SpatialVector.SIZE, MultiBodySystemTools.computeDegreesOfFreedom(joints)); this.jointPathFromBaseToEndEffector = MultiBodySystemTools.createJointPath(getBase(), getEndEffector()); this.allowChangeFrame = allowChangeFrame; hashCode = ScrewTools.computeGeometricJacobianHashCode(joints, jacobianFrame, allowChangeFrame); }
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; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPackJointVelocitiesMatrix_Iterable() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); ArrayList<JointBasics> jointsList = new ArrayList<JointBasics>(); for(int i = 0; i < jointsArray.length; i++) { jointsList.add(jointsArray[i]); } DenseMatrix64F originalVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < originalVelocities.getNumRows() * originalVelocities.getNumCols(); i++) { //create original matrix originalVelocities.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, originalVelocities); //set velocities from matrix DenseMatrix64F newVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsList, JointStateType.VELOCITY, newVelocities);//pack new matrix for(int i = 0; i < jointsArray.length; i++) { assertEquals("Should be equal velocities", originalVelocities.get(i), newVelocities.get(i), epsilon); } }
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 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); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testPackJointVelocitiesMatrix_Array() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F originalVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < originalVelocities.getNumRows() * originalVelocities.getNumCols(); i++) { //create original matrix originalVelocities.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.VELOCITY, originalVelocities); //set velocities from matrix DenseMatrix64F newVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.VELOCITY, newVelocities);//pack new matrix for(int i = 0; i < jointsArray.length; i++) { assertEquals("Should be equal velocities", originalVelocities.get(i), newVelocities.get(i), epsilon); } }
@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); } }
initialRecursionStep.includeIgnoredSubtreeInertia(); int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); jointTauMatrix = new DenseMatrix64F(nDoFs, 1); jointAccelerationMatrix = new DenseMatrix64F(nDoFs, 1);
initialRecursionStep.includeIgnoredSubtreeInertia(); int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); jointAccelerationMatrix = new DenseMatrix64F(nDoFs, 1); jointTauMatrix = new DenseMatrix64F(nDoFs, 1);
centerOfMassVelocity = new FrameVector3D(matrixFrame); int nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom); jointVelocityMatrix = new DenseMatrix64F(nDegreesOfFreedom, 1);
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBodyBasics rootBody) { SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(rootBody, false, true); idCalculator.setRootAcceleration(zeroRootAcceleration); jointsInOrder = MultiBodySystemTools.collectSubtreeJoints(rootBody); totalNumberOfDoFs = MultiBodySystemTools.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 (JointBasics joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
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; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeDegreesOfFreedom_Array() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); chain.nextState(random, JointStateType.CONFIGURATION, JointStateType.VELOCITY); RigidBodyBasics[] rootBodies = {chain.getElevator()}; JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(rootBodies); 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]; } int result = MultiBodySystemTools.computeDegreesOfFreedom(jointsArray); assertEquals(11, result); }
netCoriolisBodyWrench = new Wrench(); int nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom); jointVelocityMatrix = new DenseMatrix64F(nDegreesOfFreedom, 1);
@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 testSetDesiredAccelerations() { Vector3D[] jointAxes = {X, Y, Z, Y, X}; RandomFloatingRevoluteJointChain chain = new RandomFloatingRevoluteJointChain(random, jointAxes); JointBasics[] jointsArray = MultiBodySystemTools.collectSubtreeJoints(chain.getElevator()); DenseMatrix64F jointAccelerations = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); for(int i = 0; i < jointAccelerations.getNumRows() * jointAccelerations.getNumCols(); i++) { jointAccelerations.set(i, random.nextDouble()); } MultiBodySystemTools.insertJointsState(jointsArray, JointStateType.ACCELERATION, jointAccelerations); DenseMatrix64F sixDoFAccel = new DenseMatrix64F(6, 1); jointsArray[0].getJointAcceleration(0, sixDoFAccel); for(int i = 0; i < 6; i++) { assertEquals("Should be equal accelerations", jointAccelerations.get(i), sixDoFAccel.get(i), epsilon); } OneDoFJointBasics joint; for(int i = 6; i < jointAccelerations.getNumRows() * jointAccelerations.getNumCols(); i++) { joint = (OneDoFJointBasics)jointsArray[i - 5]; //1 - 6 assertEquals("Should be equal accelerations", jointAccelerations.get(i), joint.getQdd(), epsilon); } }
private void resizeMatrices() { int nActuatedDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(actuatedJoints); S.reshape(nActuatedDegreesOfFreedom, nDegreesOfFreedom); Js.reshape(nConstraints, nDegreesOfFreedom); AInverse.reshape(nDegreesOfFreedom, nDegreesOfFreedom); AInverseJSTranspose.reshape(AInverse.getNumRows(), Js.getNumRows()); LambdasInverse.reshape(Js.getNumRows(), AInverseJSTranspose.getNumCols()); Lambdas.reshape(LambdasInverse.getNumRows(), LambdasInverse.getNumCols()); JsBar.reshape(AInverseJSTranspose.getNumRows(), LambdasInverse.getNumCols()); Ns.reshape(nDegreesOfFreedom, nDegreesOfFreedom); SNs.reshape(S.getNumRows(), Ns.getNumCols()); AInverseSNsTranspose.reshape(AInverse.getNumRows(), SNs.getNumRows()); PhiStar.reshape(SNs.getNumRows(), AInverseSNsTranspose.getNumCols()); PhiStarInverse.reshape(PhiStar.getNumRows(), PhiStar.getNumCols()); SNsBar.reshape(nDegreesOfFreedom, nActuatedDegreesOfFreedom); }
DenseMatrix64F jointVelocities = new DenseMatrix64F(MultiBodySystemTools.computeDegreesOfFreedom(joints), 1); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocities);