public ReferenceFrame getFrame() { return geometricJacobian.getBaseFrame(); }
public ReferenceFrame getFrame() { return geometricJacobian.getBaseFrame(); }
public void set(GeometricJacobian geometricJacobian, FramePoint point) { if (geometricJacobian.getBaseFrame() != geometricJacobian.getJacobianFrame()) throw new RuntimeException("Jacobian must be expressed in base frame"); this.geometricJacobian = geometricJacobian; this.point.setIncludingFrame(point); this.point.changeFrame(geometricJacobian.getBaseFrame()); this.jacobianMatrix.reshape(3, geometricJacobian.getNumberOfColumns()); }
public void set(GeometricJacobian geometricJacobian, FramePoint3D point) { if (geometricJacobian.getBaseFrame() != geometricJacobian.getJacobianFrame()) throw new RuntimeException("Jacobian must be expressed in base frame"); this.geometricJacobian = geometricJacobian; this.point.setIncludingFrame(point); this.point.changeFrame(geometricJacobian.getBaseFrame()); this.jacobianMatrix.reshape(3, geometricJacobian.getNumberOfColumns()); }
/** * 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 void compute(PointJacobian pointJacobian, FrameVector pPointVelocity) { GeometricJacobian jacobian = pointJacobian.getGeometricJacobian(); bodyFixedPoint.setIncludingFrame(pointJacobian.getPoint()); bodyFixedPoint.changeFrame(jacobian.getBaseFrame()); twistCalculator.getRelativeTwist(twist, jacobian.getBase(), jacobian.getEndEffector()); convectiveTermCalculator.computeJacobianDerivativeTerm(jacobian, convectiveTerm); convectiveTerm.changeFrame(jacobian.getBaseFrame(), twist, twist); twist.changeFrame(jacobian.getBaseFrame()); convectiveTerm.getAccelerationOfPointFixedInBodyFrame(twist, bodyFixedPoint, pPointVelocity); // bodyFixedPointVelocity.setToZero(jacobian.getBaseFrame()); // twist.packVelocityOfPointFixedInBodyFrame(bodyFixedPointVelocity, bodyFixedPoint); // // twist.packAngularPart(tempVector); // tempVector.cross(tempVector, bodyFixedPointVelocity); // pPointVelocity.add(tempVector); } }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.invert(transformShoulderToEndEffector); transformEndEffectorToDesired.multiply(transformEndEffectorToShoulder, transformShoulderToDesired); }
throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); baseFrame = jacobian.getBaseFrame(); endEffectorFrame = jacobian.getEndEffectorFrame();
throw new RuntimeException("jacobian.getJacobianFrame() != jacobian.getEndEffectorFrame()"); baseFrame = jacobian.getBaseFrame(); endEffectorFrame = jacobian.getEndEffectorFrame();
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.setAndInvert(transformShoulderToEndEffector); transformEndEffectorToDesired.set(transformEndEffectorToShoulder); transformEndEffectorToDesired.multiply(transformShoulderToDesired); }
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 boolean isPositionReachable(int numberOfTrials, int xIndex, int yIndex, int zIndex) { boolean success = false; int counter = 0; ScrewTestTools.setRandomPositionsWithinJointLimits(robotArmJoints, random); updateRobotFrames(); voxel3dGrid.getVoxel(voxelLocation, xIndex, yIndex, zIndex); modifiableVoxelLocation.setIncludingFrame(voxelLocation); modifiableVoxelLocation.changeFrame(jacobian.getBaseFrame()); desiredEndEffectorPose.setTranslation(modifiableVoxelLocation.getX(), modifiableVoxelLocation.getY(), modifiableVoxelLocation.getZ()); while (counter < numberOfTrials) { success = linearInverseKinematicsCalculator.solve(desiredEndEffectorPose); if (success) break; ScrewTestTools.setRandomPositionsWithinJointLimits(robotArmJoints, random); updateRobotFrames(); counter++; } return success; } }
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); }
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()); }
jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame());
pointJacobian.compute(); desiredAccelerationWithRespectToBase.changeFrame(jacobian.getBaseFrame());