/** * Solves the desired transform for the joint angles using the equation: δq = J<sup>T</sup>(JJ<sup>T</sup>)<sup>-1</sup> δX, with: * <li> δq is the correction in joint space </li> * <li> J is the Jacobian </li> * <li> δX is the spatial (orientation and position) error in taskspace </li> */ @Override public boolean solve(RigidBodyTransform desiredTransform) { iterationNumber = 0; minimumErrorScalar = Double.POSITIVE_INFINITY; numberOfConstraints = inverseJacobianCalculator.getNumberOfConstraints(); boolean hasReachedMaxIterations = false; boolean hasReachedTolerance = false; do { computeError(desiredTransform); computeJointAngleCorrection(spatialError); updateBest(); applyJointAngleCorrection(); iterationNumber++; hasReachedMaxIterations = iterationNumber >= maxIterations; hasReachedTolerance = errorScalar <= tolerance; } while (!hasReachedTolerance && !hasReachedMaxIterations); updateBest(); setJointAngles(jointAnglesMinimumError); return hasReachedTolerance; }
/** * Solves the desired transform for the joint angles using the equation: δq = J<sup>T</sup>(JJ<sup>T</sup>)<sup>-1</sup> δX, with: * <li> δq is the correction in joint space </li> * <li> J is the Jacobian </li> * <li> δX is the spatial (orientation and position) error in taskspace </li> */ @Override public boolean solve(RigidBodyTransform desiredTransform) { iterationNumber = 0; minimumErrorScalar = Double.POSITIVE_INFINITY; numberOfConstraints = inverseJacobianCalculator.getNumberOfConstraints(); boolean hasReachedMaxIterations = false; boolean hasReachedTolerance = false; do { computeError(desiredTransform); computeJointAngleCorrection(spatialError); updateBest(); applyJointAngleCorrection(); iterationNumber++; hasReachedMaxIterations = iterationNumber >= maxIterations; hasReachedTolerance = errorScalar <= tolerance; } while (!hasReachedTolerance && !hasReachedMaxIterations); updateBest(); setJointAngles(jointAnglesMinimumError); return hasReachedTolerance; }
throw new RuntimeException("Invalid computed desired joint velocities: " + desiredJointVelocities.toString()); subspaceSpatialError.reshape(inverseJacobianSolver.getNumberOfConstraints(), 1); subspaceSpatialError.set(inverseJacobianSolver.getSubspaceSpatialVelocity()); CommonOps.scale(controlDT, subspaceSpatialError);