public void setControlFrameFixedInEndEffector(ReferenceFrame controlFrame) { controlFrame.getTransformToDesiredFrame(endEffectorToControlFrameOffset, jacobian.getEndEffectorFrame()); endEffectorToControlFrameOffset.invert(); }
double convergeTolerance, double acceptTolLoc, double acceptTolAngle, double parameterChangePenalty) if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); endEffectorFrame = jacobian.getEndEffectorFrame();
double convergeTolerance, double acceptTolLoc, double acceptTolAngle, double parameterChangePenalty) if (jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()) throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); endEffectorFrame = jacobian.getEndEffectorFrame();
/** * Computes and packs the twist that corresponds to the given joint velocity vector. * * @param jointVelocities the joint velocity column vector. * The vector should have the same ordering as the unit twists of the ordered joint list of this Jacobian * that were passed in during construction. * @return the twist of the end effector with respect to the base, expressed in the jacobianFrame. */ public void getTwist(DenseMatrix64F jointVelocities, Twist twistToPack) { CommonOps.mult(jacobian, jointVelocities, tempMatrix); twistToPack.set(getEndEffectorFrame(), getBaseFrame(), jacobianFrame, tempMatrix, 0); }
/** * Computes and packs the twist that corresponds to the given joint velocity vector. * * @param jointVelocities the joint velocity column vector. The vector should have the same * ordering as the unit twists of the ordered joint list of this Jacobian that were * passed in during construction. * @return the twist of the end effector with respect to the base, expressed in the * jacobianFrame. */ public void getTwist(DenseMatrix64F jointVelocities, Twist twistToPack) { CommonOps.mult(jacobian, jointVelocities, tempMatrix); twistToPack.setIncludingFrame(getEndEffectorFrame(), getBaseFrame(), jacobianFrame, 0, tempMatrix); }
public NumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double lambdaLeastSquares, 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; numberOfConstraints = SpatialVector.SIZE; numberOfDoF = jacobian.getNumberOfColumns(); inverseJacobianCalculator = InverseJacobianSolver.createInverseJacobianSolver(numberOfConstraints, numberOfDoF, false); oneDoFJoints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); jointAnglesCorrection = new DenseMatrix64F(numberOfDoF, 1); jointAnglesMinimumError = new DenseMatrix64F(numberOfDoF, 1); this.lambdaLeastSquares = lambdaLeastSquares; this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; }
public NumericalInverseKinematicsCalculator(GeometricJacobian jacobian, double lambdaLeastSquares, 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; numberOfConstraints = SpatialMotionVector.SIZE; numberOfDoF = jacobian.getNumberOfColumns(); inverseJacobianCalculator = InverseJacobianSolver.createInverseJacobianSolver(numberOfConstraints, numberOfDoF, false); oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints"); jointAnglesCorrection = new DenseMatrix64F(numberOfDoF, 1); jointAnglesMinimumError = new DenseMatrix64F(numberOfDoF, 1); this.lambdaLeastSquares = lambdaLeastSquares; this.tolerance = tolerance; this.maxIterations = maxIterations; this.maxStepSize = maxStepSize; this.minRandomSearchScalar = minRandomSearchScalar; this.maxRandomSearchScalar = maxRandomSearchScalar; }
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 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; }
RigidBodyTransform desiredTransform = jacobian.getEndEffectorFrame().getTransformToDesiredFrame(jacobian.getBaseFrame()); iterationStatistics.addValue(calculator.getNumberOfIterations()); RigidBodyTransform solvedTransform = jacobian.getEndEffectorFrame().getTransformToDesiredFrame(jacobian.getBaseFrame());
private boolean exponentialCoordinatesOK() { Twist twist = new Twist(jacobian.getEndEffectorFrame(), jacobian.getBaseFrame(), jacobian.getJacobianFrame(), error); Matrix3d rotationCheck = new Matrix3d(); rotationCheck.setIdentity(); Vector3d positionCheck = new Vector3d(); ScrewTestTools.integrate(rotationCheck, positionCheck, 1.0, twist); RigidBodyTransform transformCheck = new RigidBodyTransform(rotationCheck, positionCheck); return transformCheck.epsilonEquals(errorTransform, 1e-5); }
RigidBodyTransform desiredTransform = jacobian.getEndEffectorFrame().getTransformToDesiredFrame(jacobian.getBaseFrame()); iterationStatistics.addValue(calculator.getNumberOfIterations()); RigidBodyTransform solvedTransform = jacobian.getEndEffectorFrame().getTransformToDesiredFrame(jacobian.getBaseFrame());
private boolean exponentialCoordinatesOK() { Twist twist = new Twist(jacobian.getEndEffectorFrame(), jacobian.getBaseFrame(), jacobian.getJacobianFrame(), error); Pose3D poseCheck = new Pose3D(); new MultiBodySystemStateIntegrator(1.0).integrate(twist, poseCheck); RigidBodyTransform transformCheck = new RigidBodyTransform(poseCheck.getOrientation(), poseCheck.getPosition()); return transformCheck.epsilonEquals(errorTransform, 1e-5); }
private void computeError(RigidBodyTransform desiredTransform) { jacobian.compute(); jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.invert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); RotationTools.convertMatrixToAxisAngle(errorRotationMatrix, errorAxisAngle); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorRotationVector.scale(0.2); errorTransform.getTranslation(errorTranslationVector); MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorRotationVector, 0, 0); MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorTranslationVector, 3, 0); }
private void computeError(RigidBodyTransform desiredTransform) { jacobian.compute(); jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorRotationVector.scale(0.2); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, 0, spatialError); errorTranslationVector.get(3, 0, spatialError); }
public void computeJacobianDerivativeTerm(GeometricJacobian jacobian, SpatialAccelerationVector accelerationToPack) ReferenceFrame endEffectorFrame = jacobian.getEndEffectorFrame(); twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame);
private void computeError(RigidBodyTransform desiredTransform) { /* * B is base E is end effector D is desired end effector * * H^D_E = H^D_B * H^B_E = (H^B_D)^-1 * H^B_E * * convert rotation matrix part to rotation vector */ jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, error); errorTranslationVector.get(3, error); errorScalar = NormOps.normP2(error); assert (exponentialCoordinatesOK()); }
RigidBodyTransform newFootTransform = jacobian.getEndEffectorFrame().getTransformToDesiredFrame(jacobian.getBaseFrame()); RigidBodyTransform footInWorldTransform = jacobian.getEndEffectorFrame().getTransformToDesiredFrame(ReferenceFrame.getWorldFrame()); ReferenceFrame adjustedFootFrame = jacobian.getEndEffectorFrame();
jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame());
public void computeJacobianDerivativeTerm(SpatialAccelerationVector accelerationToPack) { ReferenceFrame endEffectorFrame = jacobian.getEndEffectorFrame(); twistOfCurrentWithRespectToBase.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); accelerationToPack.setToZero(endEffectorFrame, endEffectorFrame, endEffectorFrame); RigidBody currentBody = jacobian.getEndEffector(); for (int i = jacobian.getJointsInOrder().length - 1; i >= 0; i--) { twistOfCurrentWithRespectToBase.changeFrame(currentBody.getBodyFixedFrame()); InverseDynamicsJoint joint = jacobian.getJointsInOrder()[i]; if (currentBody == joint.getPredecessor()) { joint.getPredecessorTwist(jointTwist); currentBody = joint.getSuccessor(); } else { joint.getSuccessorTwist(jointTwist); currentBody = joint.getPredecessor(); } zeroAcceleration.setToZero(jointTwist.getBodyFrame(), jointTwist.getBaseFrame(), jointTwist.getExpressedInFrame()); zeroAcceleration.changeFrame(endEffectorFrame, twistOfCurrentWithRespectToBase, jointTwist); zeroAcceleration.add(accelerationToPack); accelerationToPack.set(zeroAcceleration); jointTwist.invert(); twistOfCurrentWithRespectToBase.add(jointTwist); } accelerationToPack.changeBodyFrameNoRelativeAcceleration(endEffectorFrame); }