@Override public void get(OneDegreeOfFreedomJoint joint) { if(!Double.isNaN(q)) { joint.setQ(q); } if(!Double.isNaN(qd)) { joint.setQd(qd); } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } }
public void updateSimulationBasedOnFullRobotModel(FloatingRootJointRobot sdfRobot) { for (OneDoFJoint joint : oneDoFJoints) { OneDegreeOfFreedomJoint simulatedJoint = (OneDegreeOfFreedomJoint) sdfRobot.getJoint(joint.getName()); simulatedJoint.setQ(joint.getqDesired()); } } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } } }
@Override public void update() { double q = jointState.getQ(); double qd = jointState.getQd(); if (!Double.isNaN(q)) { joint.setQ(q); } if (!Double.isNaN(qd)) { joint.setQd(qd); } } }
public void write() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getqDesired()); pinJoint.setQd(revoluteJoint.getQdDesired()); pinJoint.setQdd(revoluteJoint.getQddDesired()); } } }
@Override public void update() { double q = jointState.getQ(); double qd = jointState.getQd(); if (!Double.isNaN(q)) { joint.setQ(q); } if (!Double.isNaN(qd)) { joint.setQd(qd); } } }
public void write() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); } }
public void updateRobotConfigurationBasedOnJointDesiredOutputPositions() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); double q = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredPosition(); pinJoint.setQ(q); double qd = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredVelocity(); pinJoint.setQd(qd); double qdd = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredAcceleration(); pinJoint.setQdd(qdd); } }
@Override public void write() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); JointDesiredOutputReadOnly jointOutput = jointDesiredOutputListReadOnly.getJointDesiredOutput(revoluteJoint); if (jointOutput.hasDesiredPosition()) pinJoint.setQ(jointOutput.getDesiredPosition()); if (jointOutput.hasDesiredVelocity()) pinJoint.setQd(jointOutput.getDesiredVelocity()); } } }
@Override public void doControl() { for(OneDegreeOfFreedomJoint simulatedJoint : elasticJoints) { double tau = simulatedJoint.getTau(); YoDouble stiffness = jointStiffness.get(simulatedJoint); YoDouble maxDeflection = maxDeflections.get(simulatedJoint); AlphaFilteredYoVariable filteredDesired = filteredDesiredJointAngles.get(simulatedJoint); double qDesired = -MathTools.clamp(tau / stiffness.getDoubleValue(), maxDeflection.getDoubleValue()); filteredDesired.update(qDesired); simulatedJoint.setQ(filteredDesired.getDoubleValue()); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); }
public void setRobotStateToMatchFullRobotModel() { FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); fullRobotModel.updateFrames(); setRobotRootJointPositionAndOrientationToMatchFullRobotModel(sixDoFJoint, floatingJoint); setRobotRootJointVelocityAndAngularVelocityToMatchFullRobotModel(sixDoFJoint, floatingJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDegreeOfFreedomJoint.setQ(oneDoFJoint.getQ()); oneDegreeOfFreedomJoint.setQd(oneDoFJoint.getQd()); } robot.update(); }
public static void setRandomConfiguration(Random random, Robot robot) { FloatingJoint rootJoint = (FloatingJoint) robot.getRootJoints().get(0); rootJoint.setRotationAndTranslation(EuclidCoreRandomTools.nextRigidBodyTransform(random)); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) { double jointLowerLimit = Math.max(joint.getJointLowerLimit(), -2.0 * Math.PI); double jointUpperLimit = Math.min(joint.getJointUpperLimit(), 2.0 * Math.PI); double q = EuclidCoreRandomTools.nextDouble(random, jointLowerLimit, jointUpperLimit); joint.setQ(q); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setQdd(revoluteJoint.getQdd()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }
@Override public void doControl() { for(OneDegreeOfFreedomJoint simulatedJoint : elasticJoints) { double tau = simulatedJoint.getTau(); DoubleYoVariable stiffness = jointStiffness.get(simulatedJoint); DoubleYoVariable maxDeflection = maxDeflections.get(simulatedJoint); AlphaFilteredYoVariable filteredDesired = filteredDesiredJointAngles.get(simulatedJoint); double qDesired = -MathTools.clipToMinMax(tau / stiffness.getDoubleValue(), maxDeflection.getDoubleValue()); filteredDesired.update(qDesired); simulatedJoint.setQ(filteredDesired.getDoubleValue()); } }
public void updateJointPositions_ID_to_SCS() { if (scsFloatingJoint != null) { idFloatingJoint.getFrameAfterJoint().getTransformToDesiredFrame(transformToWorld, ReferenceFrame.getWorldFrame()); scsFloatingJoint.setRotationAndTranslation(transformToWorld); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setQ(idJoint.getQ()); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingInverseDynamicsJoint sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = sixDoFJoint.getJointTransform3D(); floatingJoint.setRotationAndTranslation(transform); } }
public void updateRobotConfigurationBasedOnFullRobotModel() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); } FloatingJoint floatingJoint = rootJointPair.getLeft(); FloatingJointBasics sixDoFJoint = rootJointPair.getRight(); RigidBodyTransform transform = new RigidBodyTransform(); sixDoFJoint.getJointConfiguration(transform); floatingJoint.setRotationAndTranslation(transform); } }