public RandomRestartInverseKinematicsCalculator(int maxRestarts, double restartTolerance, GeometricJacobian jacobian, NumericalInverseKinematicsCalculator inverseKinematicsCalculator) { this.inverseKinematicsCalculator = inverseKinematicsCalculator; this.joints = MultiBodySystemTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJointBasics.class); this.maxRestarts = maxRestarts; this.restartTolerance = restartTolerance; }
public RandomRestartInverseKinematicsCalculator(int maxRestarts, double restartTolerance, GeometricJacobian jacobian, NumericalInverseKinematicsCalculator inverseKinematicsCalculator) { this.inverseKinematicsCalculator = inverseKinematicsCalculator; this.joints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); this.maxRestarts = maxRestarts; this.restartTolerance = restartTolerance; }
public DesiredJointAccelerationCalculator(GeometricJacobian jacobian, LinearSolver<DenseMatrix64F> jacobianSolver) { this.jointAccelerations = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jacobian.getJointsInOrder()), 1); this.jacobian = jacobian; this.jacobianSolver = jacobianSolver; }
this.solveOrientation = solveOrientation; this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); if (oneDoFJoints.length != jacobian.getJointsInOrder().length) throw new RuntimeException("Can currently only handle OneDoFJoints");
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 void update() { for (int i = 0; i < allRigidBodies.size(); i++) { RigidBody rigidBody = allRigidBodies.get(i); FootSwitchInterface footSwitch = footSwitches.get(rigidBody); GeometricJacobian jacobian = jacobians.get(rigidBody); footSwitch.computeAndPackFootWrench(wrench); wrench.changeFrame(rigidBody.getBodyFixedFrame()); wrench.negate(); jacobian.compute(); jacobian.computeJointTorques(wrench, jointTorquesMatrix); InverseDynamicsJoint[] joints = jacobian.getJointsInOrder(); for (int j = 0; j < joints.length; j++) { OneDoFJoint joint = (OneDoFJoint) joints[j]; jointTorques.get(joint).set(jointTorquesMatrix.get(j, 0)); } } } }
public DesiredJointAccelerationCalculatorOld(GeometricJacobian jacobian, JacobianSolver jacobianSolver) { this.jointAccelerations = new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jacobian.getJointsInOrder()), 1); this.sign = ScrewTools.isAncestor(jacobian.getEndEffector(), jacobian.getBase()) ? 1 : -1; // because the sign of a joint acceleration changes when you switch a joint's 'direction' in the chain this.jacobian = jacobian; this.jacobianSolver = jacobianSolver; }
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; }
public IMUBasedJointVelocityEstimator(IMUSensorReadOnly pelvisIMU, IMUSensorReadOnly chestIMU, SensorOutputMapReadOnly sensorMap, double estimatorDT, double slopTime, YoVariableRegistry registry) { this.sensorMap = sensorMap; this.pelvisIMU = pelvisIMU; this.chestIMU = chestIMU; jacobian = new GeometricJacobian(pelvisIMU.getMeasurementLink(), chestIMU.getMeasurementLink(), chestIMU.getMeasurementLink().getBodyFixedFrame()); joints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); String namePrefix = "imuBasedJointVelocityEstimator"; alpha = new DoubleYoVariable(namePrefix + "AlphaFuse", registry); alpha.set(0.0); double dt = estimatorDT; this.slopTime = new DoubleYoVariable(namePrefix + "SlopTime", registry); this.slopTime.set(slopTime); for (OneDoFJoint joint : joints) { jointVelocitiesFromIMUOnly.put(joint, new DoubleYoVariable("qd_" + joint.getName() + "_IMUBased", registry)); jointVelocities.put(joint, new BacklashProcessingYoVariable("qd_" + joint.getName() + "_FusedWithIMU", "", dt, this.slopTime, registry)); } }
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; }
public KinematicSolver(GeometricJacobian jacobian, double tolerance, double maxIterations) { this.jacobian = jacobian; this.tolerance = tolerance; this.maxIterations = maxIterations; this.oneDoFJoints = ScrewTools.filterJoints(jacobian.getJointsInOrder(), OneDoFJoint.class); nDoF = ScrewTools.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(SpatialMotionVector.SIZE, 1); dampingSquaredDiagonal = new DenseMatrix64F(nDoF, nDoF); inverseTerm = new DenseMatrix64F(nDoF, nDoF); convergeRate = 1E-8; solveMethod = JacobianMethod.JACOBIAN_TRANSPOSE; dampingConstant = 0.3; }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); if (!jacobianSolver.setA(jacobian.getJacobianMatrix())) throw new RuntimeException("jacobian cannot be solved"); jacobianSolver.solve(biasedSpatialAcceleration, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
public static void main(String[] args) throws IOException { FramePose framePose = new FramePose(); framePose.setYawPitchRoll(1.0, 0.8, -1.1); framePose.setPosition(3.1, 0.1, 1.0); System.out.println(framePose.getFrameOrientationCopy().toStringAsQuaternion()); RigidBodyTransform transformToParent = new RigidBodyTransform(); framePose.getPose(transformToParent); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("blop", ReferenceFrame.getWorldFrame(), transformToParent); SphereVoxelShape sphereVoxelShape = new SphereVoxelShape(gridFrame, 0.1, 10, 12, SphereVoxelType.graspOrigin); Voxel3DGrid voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, 10, 0.1); RobotArm robot = new RobotArm(); OneDoFJoint[] armJoints = ScrewTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJoint.class); ReachabilityMapFileWriter reachabilityMapFileWriter = new ReachabilityMapFileWriter(robot.getName(), armJoints, voxel3dGrid, ReachabilityMapFileWriter.class); for (int i = 0; i < 70000; i++) reachabilityMapFileWriter.registerReachablePose(0, 1, 2, 3, 4); reachabilityMapFileWriter.exportAndClose(); } }
public static void main(String[] args) throws IOException { FramePose3D framePose = new FramePose3D(); framePose.setOrientationYawPitchRoll(1.0, 0.8, -1.1); framePose.setPosition(3.1, 0.1, 1.0); System.out.println(framePose.getOrientation()); RigidBodyTransform transformToParent = new RigidBodyTransform(); framePose.get(transformToParent); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("blop", ReferenceFrame.getWorldFrame(), transformToParent); SphereVoxelShape sphereVoxelShape = new SphereVoxelShape(gridFrame, 0.1, 10, 12, SphereVoxelType.graspOrigin); Voxel3DGrid voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, 10, 0.1); RobotArm robot = new RobotArm(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJointBasics.class); ReachabilityMapFileWriter reachabilityMapFileWriter = new ReachabilityMapFileWriter(robot.getName(), ReachabilityMapFileWriter.class); reachabilityMapFileWriter.initialize(armJoints, voxel3dGrid); for (int i = 0; i < 70000; i++) reachabilityMapFileWriter.registerReachablePose(0, 1, 2, 3, 4); reachabilityMapFileWriter.exportAndClose(); } }
public ReachabilitySphereMapExample() { final RobotArm robot = new RobotArm(); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); Graphics3DObject coordinate = new Graphics3DObject(); coordinate.transform(robot.getElevatorFrameTransformToWorld()); coordinate.addCoordinateSystem(1.0); scs.addStaticLinkGraphics(coordinate); scs.startOnAThread(); OneDoFJointBasics[] armJoints = MultiBodySystemTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJointBasics.class); ReachabilitySphereMapCalculator reachabilitySphereMapCalculator = new ReachabilitySphereMapCalculator(armJoints, scs); // reachabilitySphereMapCalculator.setControlFrameFixedInEndEffector(robot.getControlFrame()); ReachabilityMapListener listener = new ReachabilityMapListener() { @Override public void hasReachedNewConfiguration() { robot.copyRevoluteJointConfigurationToPinJoints(); } }; reachabilitySphereMapCalculator.attachReachabilityMapListener(listener); reachabilitySphereMapCalculator.buildReachabilitySpace(); }
private void computeJointAccelerations(SpatialAccelerationVector accelerationOfEndEffectorWithRespectToBase, SpatialAccelerationVector jacobianDerivativeTerm) { accelerationOfEndEffectorWithRespectToBase.getBodyFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBodyFrame()); accelerationOfEndEffectorWithRespectToBase.getBaseFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getBaseFrame()); accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame().checkReferenceFrameMatch(jacobianDerivativeTerm.getExpressedInFrame()); jacobian.getJacobianFrame().checkReferenceFrameMatch(accelerationOfEndEffectorWithRespectToBase.getExpressedInFrame()); accelerationOfEndEffectorWithRespectToBase.getMatrix(biasedSpatialAcceleration, 0); // unbiased at this point jacobianDerivativeTerm.getMatrix(jacobianDerivativeTermMatrix, 0); CommonOps.subtractEquals(biasedSpatialAcceleration, jacobianDerivativeTermMatrix); jacobianSolver.setJacobian(jacobian.getJacobianMatrix()); jacobianSolver.solve(jointAccelerations, biasedSpatialAcceleration); CommonOps.scale(sign, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }
public ReachabilitySphereMapExample() { final RobotArm robot = new RobotArm(); SimulationConstructionSetParameters parameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(robot, parameters); Graphics3DObject coordinate = new Graphics3DObject(); coordinate.transform(robot.getElevatorFrameTransformToWorld()); coordinate.addCoordinateSystem(1.0); scs.addStaticLinkGraphics(coordinate); scs.startOnAThread(); OneDoFJoint[] armJoints = ScrewTools.filterJoints(robot.getJacobian().getJointsInOrder(), OneDoFJoint.class); ReachabilitySphereMapCalculator reachabilitySphereMapCalculator = new ReachabilitySphereMapCalculator(armJoints, scs); reachabilitySphereMapCalculator.setControlFrameFixedInEndEffector(robot.getControlFrame()); ReachabilityMapListener listener = new ReachabilityMapListener() { @Override public void hasReachedNewConfiguration() { robot.copyRevoluteJointConfigurationToPinJoints(); } }; reachabilitySphereMapCalculator.attachReachabilityMapListener(listener); reachabilitySphereMapCalculator.buildReachabilitySpace(); }
public void compute(FrameVector desiredAngularAcceleration) { int vectorSize = SpatialMotionVector.SIZE / 2; DenseMatrix64F biasedAccelerationMatrix = new DenseMatrix64F(vectorSize, 1); DenseMatrix64F jacobianDerivativeTermMatrix = new DenseMatrix64F(vectorSize, 1); DenseMatrix64F angularJacobian = new DenseMatrix64F(SpatialMotionVector.SIZE / 2, jacobian.getNumberOfColumns()); DenseMatrix64F jointAccelerations = new DenseMatrix64F(angularJacobian.getNumCols(), 1); desiredAngularAcceleration.changeFrame(jacobian.getJacobianFrame()); desiredAngularAcceleration.getInMatrixColumn(biasedAccelerationMatrix, 0); CommonOps.scale(sign, biasedAccelerationMatrix); jacobian.compute(); SpatialAccelerationVector jacobianDerivativeTerm = new SpatialAccelerationVector(); jointAccelerationCalculator.computeJacobianDerivativeTerm(jacobianDerivativeTerm); MatrixTools.setDenseMatrixFromTuple3d(jacobianDerivativeTermMatrix, jacobianDerivativeTerm.getAngularPartCopy(), 0, 0); CommonOps.subtractEquals(biasedAccelerationMatrix, jacobianDerivativeTermMatrix); SubmatrixOps.setSubMatrix(jacobian.getJacobianMatrix(), angularJacobian, 0, 0, 0, 0, angularJacobian.getNumRows(), angularJacobian.getNumCols()); CommonOps.solve(angularJacobian, biasedAccelerationMatrix, jointAccelerations); ScrewTools.setDesiredAccelerations(jacobian.getJointsInOrder(), jointAccelerations); } }