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 FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
public JointTorqueFromForceSensorVisualizer(Map<RigidBody, FootSwitchInterface> footSwitches, YoVariableRegistry parentRegistry) { this.footSwitches = footSwitches; allRigidBodies = new ArrayList<>(footSwitches.keySet()); jacobians = new HashMap<>(); jointTorques = new HashMap<>(); for (RigidBody rigidBody : allRigidBodies) { RigidBody rootBody = ScrewTools.getRootBody(rigidBody); OneDoFJoint[] oneDoFJoints = ScrewTools.createOneDoFJointPath(rootBody, rigidBody); GeometricJacobian jacobian = new GeometricJacobian(oneDoFJoints, rigidBody.getBodyFixedFrame()); jacobians.put(rigidBody, jacobian); for (OneDoFJoint joint : oneDoFJoints) { if (!jointTorques.containsKey(joint)) { String variableName = "tau_forceSensor_" + joint.getName(); jointTorques.put(joint, new DoubleYoVariable(variableName, registry)); } } } parentRegistry.addChild(registry); }
public FourBarKinematicLoopJacobianSolver(PassiveRevoluteJoint outputJoint) { this.jointsForJacobianCalculation = getJointsForJacobianCalculation(); this.geometricJacobianToColumnJacobian = createGeometricJacobianToColumnJacobianMatrix(jointsForJacobianCalculation); this.geometricJacobianFrame = outputJoint.getFrameAfterJoint(); this.geometricJacobian = new GeometricJacobian(jointsForJacobianCalculation, geometricJacobianFrame); }
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(); }
public ThreeDoFAngularAccelerationCalculator(RigidBody base, RigidBody endEffector) { RigidBody ancestor; RigidBody descendant; if (ScrewTools.isAncestor(endEffector, base)) { ancestor = base; descendant = endEffector; sign = +1; } else { ancestor = endEffector; descendant = base; sign = -1; } this.jacobian = new GeometricJacobian(ancestor, descendant, descendant.getBodyFixedFrame()); this.jointAccelerationCalculator = new DesiredJointAccelerationCalculator(jacobian, null); }
public static NumericalInverseKinematicsCalculator createIKCalculator(JointBasics[] jointsToControl, int maxIterations) { JointBasics[] cloneOfControlledJoints = MultiBodySystemFactories.cloneKinematicChain(jointsToControl); int numberOfDoFs = cloneOfControlledJoints.length; RigidBodyBasics cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor(); ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame(); GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame); double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); }
public static NumericalInverseKinematicsCalculator createIKCalculator(InverseDynamicsJoint[] jointsToControl, int maxIterations) { InverseDynamicsJoint[] cloneOfControlledJoints = ScrewTools.cloneJointPath(jointsToControl); int numberOfDoFs = cloneOfControlledJoints.length; RigidBody cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor(); ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame(); GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame); double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); }
this.motionSubspace = new GeometricJacobian(this, unitTwistsInBodyFrame, afterJointFrame); this.motionSubspace.compute();
this.motionSubspace = new GeometricJacobian(this, unitTwistsInBodyFrame, afterJointFrame); this.motionSubspace.compute();
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)); } }
GeometricJacobian rootJacobian = new GeometricJacobian(rootBody, randomEndEffector, randomEndEffector.getBodyFixedFrame()); rootJacobian.compute(); GeometricJacobian jacobian = new GeometricJacobian(randomBase, randomEndEffector, randomEndEffector.getBodyFixedFrame()); jacobian.compute();
}; RandomFloatingRevoluteJointChain randomFloatingChain = new RandomFloatingRevoluteJointChain(random, jointAxes); GeometricJacobian jacobian = new GeometricJacobian(randomFloatingChain.getRootJoint().getSuccessor(), randomFloatingChain.getLeafBody(), randomFloatingChain.getLeafBody().getBodyFixedFrame());
public ForceBasedTouchDownDetection(FullQuadrupedRobotModel robotModel, RobotQuadrant robotQuadrant, ReferenceFrame soleFrame, YoVariableRegistry parentRegistry) { String prefix = robotQuadrant.getCamelCaseName() + name; registry = new YoVariableRegistry(prefix); isInContact = new BooleanYoVariable(prefix + "isInContact", registry); zForceThreshold = new DoubleYoVariable(prefix + "zForceThreshold", registry); measuredZForce = new DoubleYoVariable(prefix + "measuredZForce", registry); zForceThreshold.set(80.0); RigidBody body = robotModel.getPelvis(); RigidBody foot = robotModel.getFoot(robotQuadrant); footJacobian = new GeometricJacobian(body, foot, soleFrame); legOneDoFJoints = robotModel.getLegOneDoFJoints(robotQuadrant); //remove angular part MatrixTools.removeRow(selectionMatrix, 0); MatrixTools.removeRow(selectionMatrix, 0); MatrixTools.removeRow(selectionMatrix, 0); parentRegistry.addChild(registry); }
private void buildMechanismAndJacobians() { RigidBodyBasics base = new RigidBody("base", ReferenceFrame.getWorldFrame()); joint1 = new PrismaticJoint("joint1", base, new Vector3D(), new Vector3D(0.0, -1.0, 0.0)); RigidBodyBasics body1 = new RigidBody("body1", joint1, new Matrix3D(), 0.0, new Vector3D()); joint2 = new RevoluteJoint("joint2", body1, new Vector3D(0.0, 0.0, 3.0), new Vector3D(-1.0, 0.0, 0.0)); RigidBodyBasics body2 = new RigidBody("body2", joint2, new Matrix3D(), 0.0, new Vector3D()); joint3 = new PrismaticJoint("joint4", body2, new Vector3D(), new Vector3D(0.0, 0.0, 1.0)); RigidBodyBasics body3 = new RigidBody("body3", joint3, new Matrix3D(), 0.0, new Vector3D()); // create Jacobians bodyManipulatorJacobian = new GeometricJacobian(base, body3, joint3.getFrameAfterJoint()); spatialManipulatorJacobian = new GeometricJacobian(base, body3, joint1.getFrameBeforeJoint()); } }
GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, pelvis.getBodyFixedFrame()); jacobian.compute();
public ReachabilitySphereMapCalculator(OneDoFJoint[] robotArmJoints, SimulationConstructionSet scs) { this.robotArmJoints = robotArmJoints; this.scs = scs; lastJoint = robotArmJoints[robotArmJoints.length - 1]; jacobian = new GeometricJacobian(robotArmJoints, lastJoint.getSuccessor().getBodyFixedFrame()); int maxIterations = 500; spatialInverseKinematicsCalculator = createNumericalInverseKinematicsCalculator(jacobian, maxIterations, true); linearInverseKinematicsCalculator = createNumericalInverseKinematicsCalculator(jacobian, maxIterations, false); ReferenceFrame frameBeforeRootJoint = robotArmJoints[0].getFrameBeforeJoint(); RigidBodyTransform gridTransformToParent = new RigidBodyTransform(new AxisAngle4d(), new Vector3d(gridSizeInNumberOfVoxels * voxelSize / 3.0, 0.0, 0.0)); ReferenceFrame gridFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("gridFrame", frameBeforeRootJoint, gridTransformToParent); Graphics3DObject gridFrameViz = new Graphics3DObject(); gridFrameViz.transform(gridFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame())); gridFrameViz.addCoordinateSystem(1.0, YoAppearance.Blue()); scs.addStaticLinkGraphics(gridFrameViz); sphereVoxelShape = new SphereVoxelShape(gridFrame, voxelSize, numberOfRays, numberOfRotationsAroundRay, SphereVoxelType.graspOrigin); voxel3dGrid = new Voxel3DGrid(gridFrame, sphereVoxelShape, gridSizeInNumberOfVoxels, voxelSize); scs.addYoVariableRegistry(registry); }
GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, base.getBodyFixedFrame()); jacobian.compute();
private void submitAndCheckVMC(RigidBodyBasics base, RigidBodyBasics endEffector, Wrench desiredWrench, SelectionMatrix6D selectionMatrix) GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, base.getBodyFixedFrame()); jacobian.compute();
GeometricJacobian jacobian = new GeometricJacobian(controlledJoints, pelvis.getBodyFixedFrame()); jacobian.compute();