private void printPinJointInformation(OneDegreeOfFreedomJoint pinJoint, StringBuffer buffer) { buffer.append("Joint is a Pin Joint.\n"); YoDouble q = pinJoint.getQYoVariable(); buffer.append("Its q variable is named " + q.getName() + "\n"); }
public void setRobotAtPose(OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { Set<OneDoFJoint> oneDoFJoints = playbackPoseMap.keySet(); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); Double value = playbackPoseMap.get(oneDoFJoint); oneDegreeOfFreedomJoint.getQYoVariable().set(value); } }
public void setRobotAtPose(OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { Set<OneDoFJointBasics> oneDoFJoints = playbackPoseMap.keySet(); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); Double value = playbackPoseMap.get(oneDoFJoint); oneDegreeOfFreedomJoint.getQYoVariable().set(value); } }
public void setRobotAtPose(OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { Set<OneDoFJoint> oneDoFJoints = playbackPoseMap.keySet(); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); Double value = playbackPoseMap.get(oneDoFJoint); oneDegreeOfFreedomJoint.getQYoVariable().set(value); } }
public PlaybackPose(FullRobotModel fullRobotModel, OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { playbackPoseMap = new LinkedHashMap<OneDoFJoint, Double>(); OneDoFJoint[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); double jointAngle = oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue(); playbackPoseMap.put(oneDoFJoint, jointAngle); } }
public PlaybackPose(FullRobotModel fullRobotModel, OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { playbackPoseMap = new LinkedHashMap<OneDoFJointBasics, Double>(); OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); double jointAngle = oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue(); playbackPoseMap.put(oneDoFJoint, jointAngle); } }
public PlaybackPose(FullRobotModel fullRobotModel, OneDegreeOfFreedomJointHolder oneDegreeOfFreedomJointHolder) { playbackPoseMap = new LinkedHashMap<OneDoFJoint, Double>(); OneDoFJoint[] oneDoFJoints = fullRobotModel.getOneDoFJoints(); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { String jointName = oneDoFJoint.getName(); OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJointHolder.getOneDegreeOfFreedomJoint(jointName); double jointAngle = oneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue(); playbackPoseMap.put(oneDoFJoint, jointAngle); } }
@Override public void updated(long timestamp) { System.out.print(timestamp + ": "); Point3d position = new Point3d(); origin.getPosition(position); System.out.println("pos: " + position.getX() + " " + position.getY() + " " + position.getZ() + " - {"); for(OneDegreeOfFreedomJoint joint : joints) { System.out.print(joint.getQYoVariable().getDoubleValue() + ","); } System.out.println("}. height: " + desiredCoMHeight.getDoubleValue()); }
@Override public void updated(long timestamp) { System.out.print(timestamp + ": "); Point3D position = new Point3D(); origin.getPosition(position); System.out.println("pos: " + position.getX() + " " + position.getY() + " " + position.getZ() + " - {"); for(OneDegreeOfFreedomJoint joint : joints) { System.out.print(joint.getQYoVariable().getDoubleValue() + ","); } System.out.println("}. height: " + desiredCoMHeight.getDoubleValue()); }
@Override public void updated(long timestamp) { System.out.print(timestamp + ": "); Point3d position = new Point3d(); origin.getPosition(position); System.out.println("pos: " + position.getX() + " " + position.getY() + " " + position.getZ() + " - {"); for(OneDegreeOfFreedomJoint joint : joints) { System.out.print(joint.getQYoVariable().getDoubleValue() + ","); } System.out.println("}. height: " + desiredCoMHeight.getDoubleValue()); }
public void startComputation() { jointPosition.setValue(joint.getQYoVariable().getDoubleValue()); corrupt(jointPosition); jointPositionOutputPort.setData(jointPosition); }
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); }
@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); }