private void populateRefrenceFrameMap() { originalToLocalFramesMap.put(originalBaseParentJointFrame, localBaseParentJointFrame); originalToLocalFramesMap.put(originalControlFrame, localControlFrame); originalToLocalFramesMap.put(originalBaseFrame, localBaseFrame); for (int i = 0; i < numberOfDoF; i++) { OneDoFJoint originalJoint = originalJoints[i]; RigidBody originalBody = originalJoint.getSuccessor(); OneDoFJoint localJoint = localJoints[i]; RigidBody localBody = localJoint.getSuccessor(); originalToLocalFramesMap.put(originalJoint.getFrameAfterJoint(), localJoint.getFrameAfterJoint()); originalToLocalFramesMap.put(originalJoint.getFrameBeforeJoint(), localJoint.getFrameBeforeJoint()); originalToLocalFramesMap.put(originalBody.getBodyFixedFrame(), localBody.getBodyFixedFrame()); } }
private void setLocalJointAnglesToDesiredJointAngles() { for (int i = 0; i < numberOfDoF; i++) { localJoints[i].setQ(originalJoints[i].getqDesired()); localJoints[i].getFrameAfterJoint().update(); } }
private void setLocalJointAnglesToCurrentJointAngles() { for (int i = 0; i < numberOfDoF; i++) { localJoints[i].setQ(originalJoints[i].getQ()); localJoints[i].getFrameAfterJoint().update(); } }
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clipToMinMax(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
private void applyJointAngleCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; double newQ = oneDoFJoint.getQ() - jointAnglesCorrection.get(i, 0); if (limitJointAngles) newQ = Math.min(oneDoFJoint.getJointLimitUpper(), Math.max(newQ, oneDoFJoint.getJointLimitLower())); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } if (stepListener != null) { stepListener.didAnInverseKinemticsStep(errorScalar); } }
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(); } }
private void applyCorrection() { for (int i = 0; i < oneDoFJoints.length; i++) { OneDoFJoint oneDoFJoint; if (useSeed) { oneDoFJoint = oneDoFJointsSeed[i]; } else { oneDoFJoint = oneDoFJoints[i]; } double newQ = oneDoFJoint.getQ() - correction.get(i, 0); newQ = MathTools.clipToMinMax(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
this.neckReferenceFrames.put(neckJointName, fullRobotModel.getNeckJoint(neckJointName).getFrameAfterJoint()); this.spineReferenceFrames.put(spineJointName, fullRobotModel.getSpineJoint(spineJointName).getFrameAfterJoint()); this.armJointFrames.get(robotSide).put(armJointName, fullRobotModel.getArmJoint(robotSide, armJointName).getFrameAfterJoint());
leftHipPitchFrameViz.setToReferenceFrame(fullRobotModel.getLegJoint(RobotSide.LEFT, LegJointName.HIP_PITCH).getFrameAfterJoint()); FrameVector footTorque = footWrench.getAngularPartAsFrameVectorCopy(); ReferenceFrame jointFrame = fullRobotModel.getLegJoint(robotSide, LegJointName.KNEE_PITCH).getFrameAfterJoint();
desiredJointAccelerations.set(i, 0, qDotDotDesired); joint.getFrameAfterJoint().update();
public void update() { centerOfMassCalculator.getDesiredFrame().update(); centerOfMassCalculator.compute(); centerOfMassCalculator.getCenterOfMass(centerOfMassPosition); belowJointCoMInZUpFrame.set(centerOfMassPosition); jointAxis.setIncludingFrame(parentJoint.getJointAxis()); jointAxis.changeFrame(parentJoint.getFrameAfterJoint()); jointAxisInWorld.setIncludingFrame(jointAxis); jointAxisInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoJointAxis.set(jointAxisInWorld); centerOfMassPosition.changeFrame(jointAxis.getReferenceFrame()); jointToCenterOfMass.setIncludingFrame(centerOfMassPosition); jointToCenterOfMassInWorld.setIncludingFrame(jointToCenterOfMass); jointToCenterOfMassInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoJointToCenterOfMass.set(jointToCenterOfMassInWorld); totalMass.set(centerOfMassCalculator.getTotalMass()); forceVector.setIncludingFrame(ReferenceFrame.getWorldFrame(), 0.0, 0.0, -9.81 * totalMass.getDoubleValue()); forceVector.changeFrame(jointAxis.getReferenceFrame()); FrameVector forceVectorInWorld = new FrameVector(forceVector); forceVectorInWorld.changeFrame(ReferenceFrame.getWorldFrame()); yoForceVector.set(forceVectorInWorld); rCrossFVector.setToZero(jointAxis.getReferenceFrame()); rCrossFVector.cross(forceVector, jointToCenterOfMass); estimatedTorque.set(rCrossFVector.dot(jointAxis)); if (isSpineJoint) estimatedTorque.mul(-1.0); }
tempPoint.changeFrame(armJoints[1].getFrameAfterJoint()); FrameVector tempVector = new FrameVector(tempPoint); MathTools.floorToGivenPrecision(tempVector.getVector(), 1.0e-2);