public void startComputation() { jointVelocity.setValue(joint.getQDYoVariable().getDoubleValue()); corrupt(jointVelocity); jointVelocityOutputPort.setData(jointVelocity); }
@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() { 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); }
private void createAndAddOneDoFPositionAndVelocitySensors(SCSToInverseDynamicsJointMap scsToInverseDynamicsJointMap) { ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(scsToInverseDynamicsJointMap.getSCSOneDegreeOfFreedomJoints()); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(oneDegreeOfFreedomJoint); simulatedSensorHolderAndReader.addJointPositionSensorPort(oneDoFJoint, oneDegreeOfFreedomJoint.getQYoVariable()); simulatedSensorHolderAndReader.addJointVelocitySensorPort(oneDoFJoint, oneDegreeOfFreedomJoint.getQDYoVariable()); simulatedSensorHolderAndReader.addJointTorqueSensorPort(oneDoFJoint, oneDegreeOfFreedomJoint.getTauYoVariable()); } }
@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); }
private void readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); if (pinJoint == null) continue; revoluteJoint.setQ(pinJoint.getQYoVariable().getDoubleValue()); revoluteJoint.setQd(pinJoint.getQDYoVariable().getDoubleValue()); revoluteJoint.setQdd(pinJoint.getQDDYoVariable().getDoubleValue()); } }
YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, joint.getQYoVariable(), robot.getYoTime(), valueDataCheckerParameters, joint.getQDYoVariable());
private void readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations() { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJointBasics revoluteJoint = jointPair.getRight(); if (pinJoint == null) continue; revoluteJoint.setQ(pinJoint.getQYoVariable().getDoubleValue()); revoluteJoint.setQd(pinJoint.getQDYoVariable().getDoubleValue()); revoluteJoint.setQdd(pinJoint.getQDDYoVariable().getDoubleValue()); revoluteJoint.setTau(pinJoint.getTau()); } }
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); } }
public void updateJointVelocities_SCS_to_ID() { if (scsFloatingJoint != null) { ReferenceFrame elevatorFrame = idFloatingJoint.getFrameBeforeJoint(); ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint(); scsFloatingJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(rootBodyFrame); scsFloatingJoint.getAngularVelocity(angularVelocity, rootBodyFrame); rootJointTwist.setIncludingFrame(rootBodyFrame, elevatorFrame, rootBodyFrame, angularVelocity, linearVelocity); idFloatingJoint.setJointTwist(rootJointTwist); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue()); } }
public void setRobotStateToMatchOtherRobot(FloatingRootJointRobot otherRobot) { otherRobot.update(); FloatingJoint floatingJoint = robot.getRootJoint(); FloatingJoint otherFloatingJoint = otherRobot.getRootJoint(); Tuple3DBasics position = new Point3D(); Tuple3DBasics velocity = new Vector3D(); otherFloatingJoint.getPositionAndVelocity(position, velocity); floatingJoint.setPositionAndVelocity(position, velocity); Quaternion rotation = new Quaternion(); otherFloatingJoint.getQuaternion(rotation); floatingJoint.setQuaternion(rotation); Vector3D angularVelocityInBody = otherFloatingJoint.getAngularVelocityInBody(); floatingJoint.setAngularVelocityInBody(angularVelocityInBody); 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); oneDegreeOfFreedomJoint.setQ(otherOneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDegreeOfFreedomJoint.setQd(otherOneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
@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 setFullRobotModelStateToMatchRobot() { robot.update(); FloatingJointBasics sixDoFJoint = fullRobotModel.getRootJoint(); FloatingJoint floatingJoint = robot.getRootJoint(); setFullRobotModelRootJointPositionAndOrientationToMatchRobot(sixDoFJoint, floatingJoint); fullRobotModel.updateFrames(); setFullRobotModelRootJointVelocityAndAngularVelocityToMatchRobot(sixDoFJoint, floatingJoint); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); oneDoFJoint.setQ(oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDoFJoint.setQd(oneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
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); }
double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue();
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription, false, false); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription, false, false); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }