private static void copyTorques(HashMap<RevoluteJoint, PinJoint> jointMap) { for (RevoluteJoint idJoint : jointMap.keySet()) { OneDegreeOfFreedomJoint joint = jointMap.get(idJoint); double tau = idJoint.getTau(); // System.out.println("tau = " + tau); joint.setTau(tau); } }
public void updateJointTorques_ID_to_SCS() { for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setTau(idJoint.getTau()); } }
@Override public void doControl() { for (int i = 0; i < joints.size(); i++) { double torque = amplitudes.get(i) * Math.sin(t.getDoubleValue() * omegas.get(i)); ((OneDegreeOfFreedomJoint) joints.get(i)).setTau(torque); } } }
public static void setRandomEffort(Random random, Robot robot) { ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) joint.setTau(EuclidCoreRandomTools.nextDouble(random)); }
public void setRobotTorquesRandomly(Random random, double maxJointTorque) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { oneDegreeOfFreedomJoint.setTau(RandomNumbers.nextDouble(random, maxJointTorque)); } }
@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.setTau(revoluteJoint.getTau()); } }
public void setRobotTorquesToMatchOtherRobot(FloatingRootJointRobot otherRobot) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); if (!oneDegreeOfFreedomJoint.getName().equals(otherOneDegreeOfFreedomJoint.getName())) { throw new RuntimeException(); } oneDegreeOfFreedomJoint.setTau(otherOneDegreeOfFreedomJoint.getTauYoVariable().getDoubleValue()); } }
@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(); double tau; if (jointDesiredOutputList != null) tau = jointDesiredOutputList.getJointDesiredOutput(revoluteJoint).getDesiredTorque(); else tau = revoluteJoint.getTau(); pinJoint.setTau(tau); } }
public void setRobotTorquesToMatchFullRobotModelButCheckAgainstOtherRobot(FloatingRootJointRobot otherRobot, double epsilon) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); double inverseDynamicsTorque = oneDoFJoint.getTau(); oneDegreeOfFreedomJoint.setTau(inverseDynamicsTorque); YoDouble computedJointTorque = computedJointTorques.get(oneDoFJoint); computedJointTorque.set(inverseDynamicsTorque); double otherRobotJointTorque = otherOneDegreeOfFreedomJoint.getTauYoVariable().getDoubleValue(); YoDouble differenceJointTorque = differenceJointTorques.get(oneDoFJoint); differenceJointTorque.set(otherRobotJointTorque - inverseDynamicsTorque); if (Math.abs(differenceJointTorque.getDoubleValue()) > epsilon) { System.err.println("Torques don't match. oneDegreeOfFreedomJoint = " + oneDegreeOfFreedomJoint.getName() + ", otherRobotJointTorque = " + otherRobotJointTorque + ", inverseDynamicsTorque = " + inverseDynamicsTorque); } } }
public void doControl() { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = 0.0; double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = 0.0; double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate); simulatedJoint.setTau(desiredTau); }
@Override public void doControl() { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = 0.0; double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = 0.0; double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate); simulatedJoint.setTau(desiredTau); }
@Override public void doControl() { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = 0.0; double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = 0.0; double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate); simulatedJoint.setTau(desiredTau); }
public void doControl() { if (highLevelControllerOutputJoint.isUnderPositionControl()) { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = highLevelControllerOutputJoint.getqDesired(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = highLevelControllerOutputJoint.getQdDesired(); double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau); } }
@Override public void doControl() { if (highLevelControllerOutputJoint.isUnderPositionControl()) { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = highLevelControllerOutputJoint.getqDesired(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = highLevelControllerOutputJoint.getQdDesired(); double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate, controlDT); simulatedJoint.setTau(desiredTau); } }
@Override public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
@Override public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
public void doControl() { desiredLidarAngle.add(desiredLidarVelocity.getDoubleValue() * controlDT); double lidarJointTau = lidarJointController.compute(lidarJoint.getQYoVariable().getDoubleValue(), desiredLidarAngle.getDoubleValue(), lidarJoint.getQDYoVariable() .getDoubleValue(), desiredLidarVelocity.getDoubleValue(), controlDT) + lidarJoint.getDamping() * desiredLidarVelocity.getDoubleValue(); lidarJoint.setTau(lidarJointTau); }
public void update(long timestamp) { fullRobot.updateFrames(); if(rootJoint != null) { RigidBodyTransform rootTransform = rootJoint.getJointTransform3D(); rootTransform.get(tempOrientation, tempPosition); robot.setOrientation(tempOrientation); robot.setPositionInWorld(tempPosition); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }
@Override public void update(long timestamp) { fullRobot.updateFrames(); latestTimestamp = timestamp; if(rootJoint != null) { robot.setOrientation(rootJoint.getJointPose().getOrientation()); robot.setPositionInWorld(rootJoint.getJointPose().getPosition()); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }
protected void write() { for (int i = 0; i < revoluteJoints.size(); i++) { OneDegreeOfFreedomJoint pinJoint = revoluteJoints.first(i); JointDesiredOutputReadOnly data = revoluteJoints.second(i); if(data.hasDesiredTorque()) { pinJoint.setTau(data.getDesiredTorque()); } if (data.hasStiffness()) { pinJoint.setKp(data.getStiffness()); } if (data.hasDamping()) { pinJoint.setKd(data.getDamping()); } if (data.hasDesiredPosition()) { pinJoint.setqDesired(data.getDesiredPosition()); } if (data.hasDesiredVelocity()) { pinJoint.setQdDesired(data.getDesiredVelocity()); } } }