@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 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 final Limit createJointLimit(OneDegreeOfFreedomJoint scsJoint) { Limit sdfJointLimit = new Limit(); String effort = String.valueOf(scsJoint.getTorqueLimit()); sdfJointLimit.setEffort(effort); String velocity = String.valueOf(scsJoint.getVelocityLimit()); sdfJointLimit.setVelocity(velocity); String lower = String.valueOf(scsJoint.getJointLowerLimit()); sdfJointLimit.setLower(lower); String upper = String.valueOf(scsJoint.getJointUpperLimit()); sdfJointLimit.setUpper(upper); double jointRange = scsJoint.getJointUpperLimit() - scsJoint.getJointLowerLimit(); if (jointRange == 0.0) PrintTools.debug(this, scsJoint.getName() + " upper joint limit equals lower joint limit!"); return sdfJointLimit; }
@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 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()); } }
nextJointA.setLink(nextLink(seed)); nextJointB.setLink(nextLink(seed)); oneDoFJointsA[jointIndex].setTau(tau); oneDoFJointsB[jointIndex].setQ(oneDoFJointsA[jointIndex].getQ()); oneDoFJointsB[jointIndex].setQd(oneDoFJointsA[jointIndex].getQD()); oneDoFJointsB[jointIndex].setQdd(oneDoFJointsA[jointIndex].getQDD()); oneDoFJointsA[jointIndex].setTau(tau); oneDoFJointsB[jointIndex].setTau(tau); assertEquals(oneDoFJointsA[jointIndex].getQ(), oneDoFJointsB[jointIndex].getQ(), EPSILON); assertEquals(oneDoFJointsA[jointIndex].getQD(), oneDoFJointsB[jointIndex].getQD(), EPSILON); assertEquals(oneDoFJointsA[jointIndex].getQDD(), oneDoFJointsB[jointIndex].getQDD(), EPSILON);
throw new RuntimeException("Joint " + simulatedJoint.getName() + " cannot be in position control mode without a valid position."); double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = actuatorDesireds.getPosition(); if (actuatorDesireds.isVelocityValid()) desiredRate = actuatorDesireds.getVelocity(); double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); case VELOCITY: if (!actuatorDesireds.isVelocityValid()) throw new RuntimeException("Joint " + simulatedJoint.getName() + " cannot be in velocity control mode without a valid velocity."); currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); desiredRate = actuatorDesireds.getVelocity(); break; case EFFORT: currentPosition = simulatedJoint.getQ(); desiredPosition = actuatorDesireds.isPositionValid() ? actuatorDesireds.getPosition() : currentPosition; currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); desiredRate = actuatorDesireds.isVelocityValid() ? actuatorDesireds.getVelocity() : 0.0; throw new RuntimeException("This hasn't been implemented yet."); simulatedJoint.setTau(outputEffort);
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(); } }
double upperLimit = joint.getJointUpperLimit(); double lowerLimit = joint.getJointLowerLimit(); double range = upperLimit - lowerLimit; valueDataCheckerParameters.setMinimumValue(lowerLimit - limitAdjustment); valueDataCheckerParameters.setMaximumDerivative((1.0 + TOLERANCE_FACTOR)* joint.getVelocityLimit()); valueDataCheckerParameters.setMaximumSecondDerivative(joint.getVelocityLimit()/ MINIMUM_TIME_TO_ACCLERATE); YoVariableValueDataChecker yoVariableValueDataChecker = new YoVariableValueDataChecker(scs, joint.getQYoVariable(), robot.getYoTime(), valueDataCheckerParameters, joint.getQDYoVariable());
for (OneDegreeOfFreedomJoint pinJoint : pinJoints) if (updateRootJoints || (pinJoint.getParentJoint() != null)) pinJoint.setQ(jointPosition); pinJoint.setQdd(jointAcceleration); pinJoint.setQd(jointVelocity); pinJoint.setTau(jointTorque);
@Override public void writeAfterController(long timestamp) { for (int i = 0; i < revoluteJoints.size(); i++) { ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = revoluteJoints.get(i); OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); double tau = revoluteJoint.getTau(); DoubleYoVariable rawJointTorque = rawJointTorques.get(revoluteJoint); DelayedDoubleYoVariable delayedJointTorque = delayedJointTorques.get(revoluteJoint); if (rawJointTorque != null) { rawJointTorque.set(tau); delayedJointTorque.update(); tau = delayedJointTorque.getDoubleValue(); } pinJoint.setTau(tau); pinJoint.setKp(revoluteJoint.getKp()); pinJoint.setKd(revoluteJoint.getKd()); pinJoint.setqDesired(revoluteJoint.getqDesired()); pinJoint.setQdDesired(revoluteJoint.getQdDesired()); } for (int i = 0; i < rawOutputWriters.size(); i++) { rawOutputWriters.get(i).write(); } }
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 setRobotStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJoint rootJoint = robot.getRootJoint(); rootJoint.setVelocity(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setAngularVelocityInBody(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.setYawPitchRoll(yaw, pitch, roll); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { double lowerLimit = oneDegreeOfFreedomJoint.getJointLowerLimit(); double upperLimit = oneDegreeOfFreedomJoint.getJointUpperLimit(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDegreeOfFreedomJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDegreeOfFreedomJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
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 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()); } }
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 updateSimulationBasedOnFullRobotModel(FloatingRootJointRobot sdfRobot) { for (OneDoFJoint joint : oneDoFJoints) { OneDegreeOfFreedomJoint simulatedJoint = (OneDegreeOfFreedomJoint) sdfRobot.getJoint(joint.getName()); simulatedJoint.setQ(joint.getqDesired()); } } }
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()); } }
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()); } }