/** * Creates a new Jacobian for the open loop chain described by the ordered keys of {@code unitTwists}. * * @param unitTwists an ordered list of relative joint twists. * The order in which the twists are ordered will correspond to the order of the columns in the Jacobian * @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. */ private GeometricJacobian(LinkedHashMap<InverseDynamicsJoint, List<Twist>> unitTwists, ReferenceFrame jacobianFrame, boolean allowChangeFrame) { this.joints = new InverseDynamicsJoint[unitTwists.size()]; unitTwists.keySet().toArray(joints); this.unitTwistMap = unitTwists; this.unitTwistList = extractUnitTwistList(unitTwists); this.jacobianFrame = jacobianFrame; this.jacobian = createJacobianMatrix(this.unitTwistMap); this.jointPathFromBaseToEndEffector = ScrewTools.createJointPath(getBase(), getEndEffector()); this.allowChangeFrame = allowChangeFrame; nameBasedHashCode = ScrewTools.computeGeometricJacobianNameBasedHashCode(joints, jacobianFrame, allowChangeFrame); }
private long getOrCreateGeometricJacobian(InverseDynamicsJoint[] joints, int numberOfJointsToConsider, ReferenceFrame jacobianFrame) { if (joints == null || numberOfJointsToConsider == 0) return NULL_JACOBIAN_ID; // The mapping assumes the frame do not change. // On top of that, this class makes the different modules use the same instances of each Jacobian, so it would not be good if one module changes the frame of a Jacobian shared with another module. boolean allowChangeFrame = false; long jacobianId = ScrewTools.computeGeometricJacobianNameBasedHashCode(joints, 0, numberOfJointsToConsider - 1, jacobianFrame, allowChangeFrame); GeometricJacobian jacobian = getJacobian(jacobianId); if (jacobian == null) { if (joints.length == numberOfJointsToConsider) { jacobian = new GeometricJacobian(joints, jacobianFrame, allowChangeFrame); } else { InverseDynamicsJoint[] jointsForNewJacobian = new InverseDynamicsJoint[numberOfJointsToConsider]; System.arraycopy(joints, 0, jointsForNewJacobian, 0, numberOfJointsToConsider); jacobian = new GeometricJacobian(jointsForNewJacobian, jacobianFrame, allowChangeFrame); } jacobian.compute(); // Compute in case you need it right away geometricJacobians.add(jacobian); nameBasedHashCodeToJacobianMap.put(jacobian.nameBasedHashCode(), jacobian); } return jacobian.nameBasedHashCode(); }