@Override public void setJointPositionVelocityAndAcceleration(InverseDynamicsJoint originalJoint) { OneDoFJoint oneDoFOriginalJoint = checkAndGetAsOneDoFJoint(originalJoint); setQ(oneDoFOriginalJoint.getQ()); setQd(oneDoFOriginalJoint.getQd()); setQdd(oneDoFOriginalJoint.getQdd()); setTauMeasured(oneDoFOriginalJoint.getTauMeasured()); setEnabled(oneDoFOriginalJoint.isEnabled()); }
private static OneDoFJoint cloneOneDoFJoint(OneDoFJoint original, String cloneSuffix, RigidBody clonePredecessor) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = original.getOffsetTransform3D(); Vector3d jointAxisCopy = original.getJointAxis().getVectorCopy(); OneDoFJoint clone; if (original instanceof RevoluteJoint) clone = ScrewTools.addRevoluteJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else if (original instanceof PrismaticJoint) clone = ScrewTools.addPrismaticJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else throw new RuntimeException("Unhandled type of " + OneDoFJoint.class.getSimpleName() + ": " + original.getClass().getSimpleName()); clone.setJointLimitLower(original.getJointLimitLower()); clone.setJointLimitUpper(original.getJointLimitUpper()); return clone; }
@Override public void updateAnkleState(OneDoFJoint ankleX, OneDoFJoint ankleY) { q_AnkleX = ankleX.getQ(); q_AnkleY = ankleY.getQ(); measuredJointVelocities.set(0,ankleX.getQd()); measuredJointVelocities.set(1,ankleY.getQd()); desiredJointTorque.set(0,ankleX.getTau()); desiredJointTorque.set(1,ankleY.getTau()); updateAnkleStateFromJointAngles(q_AnkleX, q_AnkleY); computeDesiredAcutatorTau(); }
public JointLimit(OneDoFJoint oneDoFJoint) { upperPositionLimit = oneDoFJoint.getJointLimitUpper(); lowerPositionLimit = oneDoFJoint.getJointLimitLower(); rangeOfMotion = upperPositionLimit - lowerPositionLimit; softLimitPercentageOfFullRangeOfMotion = 1.0; softUpperPositionLimit = upperPositionLimit; softLowerPositionLimit = lowerPositionLimit; softLimitRangeOfMotion = softUpperPositionLimit - softLowerPositionLimit; velocityLimit = Double.POSITIVE_INFINITY; torqueLimit = Double.POSITIVE_INFINITY; }
private void updateJointAngles() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = oneDoFJoints[i].getQ() + correction.get(i, 0); newQ = MathTools.clipToMinMax(newQ, oneDoFJoints[i].getJointLimitLower(), oneDoFJoints[i].getJointLimitUpper()); oneDoFJoints[i].setQ(newQ); oneDoFJoints[i].getFrameAfterJoint().update(); } }
double q = jointsOriginal[i].getQ(); double qd = jointsOriginal[i].getQd(); jointAtDesiredPosition.setQ(qDesired); jointAtDesiredPosition.setQd(qdDesired); jointAtDesiredPosition.setQ(q); jointAtDesiredPosition.setQd(qd); jointsAtDesiredPosition[i].setQ(jointspaceHeadControlState.getJointDesiredPosition(jointsOriginal[i])); jointsAtDesiredPosition[i].setQd(jointspaceHeadControlState.getJointDesiredVelocity(jointsOriginal[i])); jointsAtDesiredPosition[i].setQ(jointsOriginal[i].getQ()); jointsAtDesiredPosition[i].setQd(jointsOriginal[i].getQd()); jointsAtDesiredPosition[0].updateFramesRecursively();
private void setLocalJointAnglesToCurrentJointAngles() { for (int i = 0; i < numberOfDoF; i++) { localJoints[i].setQ(originalJoints[i].getQ()); localJoints[i].getFrameAfterJoint().update(); } }
public void setDesiredsFromOneDoFJoint(OneDoFJoint jointToExtractDesiredsFrom) { setDesiredTorque(jointToExtractDesiredsFrom.getTau()); setDesiredPosition(jointToExtractDesiredsFrom.getqDesired()); setDesiredVelocity(jointToExtractDesiredsFrom.getQdDesired()); setDesiredAcceleration(jointToExtractDesiredsFrom.getQddDesired()); setResetIntegrators(jointToExtractDesiredsFrom.getResetIntegrator()); }
private void updateDesiredFullRobotModelState() { RootJointDesiredConfigurationDataReadOnly outputForRootJoint = wholeBodyInverseKinematicsSolver.getOutputForRootJoint(); desiredRootJoint.setConfiguration(outputForRootJoint.getDesiredConfiguration(), 0); desiredRootJoint.setVelocity(outputForRootJoint.getDesiredVelocity(), 0); LowLevelOneDoFJointDesiredDataHolderReadOnly output = wholeBodyInverseKinematicsSolver.getOutput(); for (OneDoFJoint joint : oneDoFJoints) { if (output.hasDataForJoint(joint)) { joint.setQ(output.getDesiredJointPosition(joint)); joint.setqDesired(output.getDesiredJointPosition(joint)); joint.setQd(output.getDesiredJointVelocity(joint)); } } }
double jointSign = -Math.signum(elbowJoint.getJointLimitLower() + elbowJoint.getJointLimitUpper()); elbowJointSign.put(robotSide, jointSign); RigidBody upperArmBody = elbowJoint.getPredecessor(); RigidBody lowerArmBody = elbowJoint.getSuccessor(); tempPoint.changeFrame(armJoints[1].getFrameAfterJoint()); FrameVector tempVector = new FrameVector(tempPoint); MathTools.floorToGivenPrecision(tempVector.getVector(), 1.0e-2); FrameVector elbowJointAxis = elbowJoint.getJointAxis(); zRotationDueToAccountForElbowAxis.setRotationPitchAndZeroTranslation(-elbowJointAxis.angle(expectedElbowAxis)); armZeroJointAngleConfigurationOffset.multiply(zRotationDueToAccountForElbowAxis); upperArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(upperArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForUpperArm = new NumericalInverseKinematicsCalculator(upperArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); lowerArmJointsClone.put(robotSide, ScrewTools.filterJoints(ScrewTools.cloneJointPath(lowerArmJoints.get(robotSide)), OneDoFJoint.class)); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide), lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length - 1].getSuccessor().getBodyFixedFrame()); NumericalInverseKinematicsCalculator inverseKinematicsForLowerArm = new NumericalInverseKinematicsCalculator(lowerArmJacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar);
private void introduceRandomPose() { for (int i = 0; i < oneDoFJoints.length; i++) { double newQ = generateRandomDoubleInRange(oneDoFJoints[i].getJointLimitUpper(), oneDoFJoints[i].getJointLimitLower()); oneDoFJoints[i].setQ(newQ); } }
public void setJointState(ArrayList<OneDoFJoint> newJointData) { if (newJointData.size() != jointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < jointAngles.length; i++) { jointAngles[i] = (float) newJointData.get(i).getQ(); jointVelocities[i] = (float) newJointData.get(i).getQd(); jointTorques[i] = (float) newJointData.get(i).getTauMeasured(); } }
public void startComputation() { for (OneDoFJoint joint : oneDoFJoints) { if (joint == null) throw new RuntimeException(); ControlFlowInputPort<double[]> positionSensorPort = positionSensorInputPorts.get(joint); ControlFlowInputPort<double[]> velocitySensorPort = velocitySensorInputPorts.get(joint); double positionSensorData = positionSensorPort.getData()[0]; double velocitySensorData = velocitySensorPort.getData()[0]; joint.setQ(positionSensorData); joint.setQd(velocitySensorData); joint.setQdd(joint.getQddDesired()); } // TODO: Does it make sense to do this yet if the orientation of the pelvis isn't known yet? FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureOutputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); twistCalculator.getRootBody().updateFramesRecursively(); twistCalculator.compute(); spatialAccelerationCalculator.compute(); inverseDynamicsStructureOutputPort.setData(inverseDynamicsStructure); }
originalControlFrame = endEffector.getBodyFixedFrame(); localEndEffectorFrame = localJoints[numberOfDoF - 1].getSuccessor().getBodyFixedFrame(); localControlFrame = createLocalControlFrame(localEndEffectorFrame, originalEndEffectorFrame); String jointName = originalJoints[i].getName(); yoPrivilegedJointPositions[i] = new DoubleYoVariable("q_privileged_" + jointName, registry); yoPrivilegedJointPositionsFiltered[i] = new AlphaFilteredYoVariable("q_privileged_filt_" + jointName, registry, 0.99, yoPrivilegedJointPositions[i]); jointSquaredRangeOfMotions.set(i, 0, MathTools.square(localJoints[i].getJointLimitUpper() - localJoints[i].getJointLimitLower())); jointAnglesAtMidRangeOfMotion.set(i, 0, 0.5 * (localJoints[i].getJointLimitUpper() + localJoints[i].getJointLimitLower()));