public void compute() { for (int i = 0; i < geometricJacobians.size(); i++) { geometricJacobians.get(i).compute(); } }
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)); } } } }
private DenseMatrix64F getUpdatedJacobianMatrix() { jacobian.compute(); return jacobian.getJacobianMatrix(); }
private DenseMatrix64F getUpdatedJacobianMatrix() { jacobian.compute(); return jacobian.getJacobianMatrix(); }
protected void setMotionSubspace(Twist unitTwist) { ArrayList<Twist> unitTwists = new ArrayList<Twist>(); unitTwists.add(unitTwist); this.motionSubspace = new GeometricJacobian(this, unitTwists, unitTwist.getExpressedInFrame()); this.motionSubspace.compute(); }
public double computeDeterminant() { jacobian.compute(); return inverseJacobianSolver.computeDeterminant(jacobian.getJacobianMatrix()); }
public void getUnitJacobian(DenseMatrix64F unitJacobianToPack) { // Geometric Jacobian - open kinematic chain Jacobian geometricJacobian.compute(); // Vector to go from open loop to closed loop Jacobian computeVectorTransformGeometricToColumnJacobian(); // Column Jacobian - fourbars are a 1DOF system so the Jacobian is a column vector CommonOps.mult(geometricJacobian.getJacobianMatrix(), geometricJacobianToColumnJacobian, unitJacobianToPack); }
public void compute(FramePose desiredPose, Twist desiredTwist) { jacobian.compute(); desiredControlFramePose.setPoseIncludingFrame(desiredPose); desiredControlFrameTwist.set(desiredTwist); computeJointAnglesAndVelocities(desiredControlFramePose, desiredControlFrameTwist); }
public void getUnitJacobian(DenseMatrix64F unitJacobianToPack) { // Geometric Jacobian - open kinematic chain Jacobian geometricJacobian.compute(); geometricJacobian.changeFrame(geometricJacobianFrame); // Vector to go from open loop to closed loop Jacobian computeVectorTransformGeometricToColumnJacobian(); // Column Jacobian - fourbars are a 1DOF system so the Jacobian is a column vector CommonOps.mult(geometricJacobian.getJacobianMatrix(), geometricJacobianToColumnJacobian, unitJacobianToPack); }
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(); }
this.motionSubspace.compute();
@Override public void update() { for(int i = 0; i < legOneDoFJoints.size(); i++) { OneDoFJoint oneDoFJoint = legOneDoFJoints.get(i); jointTorques.set(i, 0, oneDoFJoint.getTauMeasured()); } footJacobian.compute(); DenseMatrix64F jacobianMatrix = footJacobian.getJacobianMatrix(); CommonOps.mult(selectionMatrix, jacobianMatrix, linearPartOfJacobian); UnrolledInverseFromMinor.inv3(linearPartOfJacobian, linearJacobianInverse, 1.0); CommonOps.multTransA(linearJacobianInverse, jointTorques, footLinearForce); footForce.setToZero(footJacobian.getJacobianFrame()); footForce.set(footLinearForce.get(0), footLinearForce.get(1), footLinearForce.get(2)); footForce.changeFrame(ReferenceFrame.getWorldFrame()); measuredZForce.set(footForce.getZ() * -1.0); isInContact.set(measuredZForce.getDoubleValue() > zForceThreshold.getDoubleValue()); } }
public void compute() { jacobian.compute(); CommonOps.extract(jacobian.getJacobianMatrix(), 0, 3, 0, 3, jacobianAngularPart64F, 0, 0); if (Math.abs(CommonOps.det(jacobianAngularPart64F)) < 1e-5) return; CommonOps.invert(jacobianAngularPart64F, inverseAngularJacobian64F); chestAngularVelocity.setToZero(chestIMU.getMeasurementFrame()); chestIMU.getAngularVelocityMeasurement(chestAngularVelocity.getVector()); chestAngularVelocity.changeFrame(jacobian.getJacobianFrame()); pelvisAngularVelocity.setToZero(pelvisIMU.getMeasurementFrame()); pelvisIMU.getAngularVelocityMeasurement(pelvisAngularVelocity.getVector()); pelvisAngularVelocity.changeFrame(jacobian.getJacobianFrame()); chestAngularVelocity.sub(pelvisAngularVelocity); omega.setData(chestAngularVelocity.toArray()); CommonOps.mult(inverseAngularJacobian64F, omega, qd_estimated); for (int i = 0; i < joints.length; i++) { OneDoFJoint joint = joints[i]; double qd_sensorMap = sensorMap.getJointVelocityProcessedOutput(joint); double qd_IMU = qd_estimated.get(i, 0); double qd_fused = (1.0 - alpha.getDoubleValue()) * qd_sensorMap + alpha.getDoubleValue() * qd_IMU; jointVelocitiesFromIMUOnly.get(joint).set(qd_IMU); jointVelocities.get(joint).update(qd_fused); } }
private void computeCorrection() { jacobian.compute(); DenseMatrix64F dampenedJ = jacobian.getJacobianMatrix().copy(); for (int i = 0; i < dampenedJ.getNumCols(); i++) { dampenedJ.add(i, i, 0.0); } // solver.setAlpha(errorScalar + dampedLeastSquaresAlphaMin); if (!solver.setA(dampenedJ)) { // do something here intelligent if it fails // System.err.println("IK solver internal solve failed!"); } solver.solve(error, correction); double correctionScale = RandomTools.generateRandomDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, correction); for (int i = 0; i < correction.getNumRows(); i++) { correction.set(i, 0, MathTools.clipToMinMax(correction.get(i, 0), -maxStepSize, maxStepSize)); } }
private void computeCorrection() { jacobian.compute(); DenseMatrix64F dampenedJ = jacobian.getJacobianMatrix().copy(); for (int i = 0; i < dampenedJ.getNumCols(); i++) { dampenedJ.add(i, i, 0.0); } // solver.setAlpha(errorScalar + dampedLeastSquaresAlphaMin); if (!solver.setA(dampenedJ)) { // do something here intelligent if it fails // System.err.println("IK solver internal solve failed!"); } solver.solve(error, correction); double correctionScale = RandomNumbers.nextDouble(random, minRandomSearchScalar, maxRandomSearchScalar); CommonOps.scale(correctionScale, correction); for (int i = 0; i < correction.getNumRows(); i++) { correction.set(i, 0, MathTools.clamp(correction.get(i, 0), -maxStepSize, maxStepSize)); } }
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 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); } }