private FloatingInverseDynamicsJoint getRootSixDoFJoint() { ArrayList<Joint> rootJoints = robot.getRootJoints(); if (rootJoints.size() > 1) throw new RuntimeException("Only works with one root joint"); FloatingJoint rootJoint = (FloatingJoint) rootJoints.get(0); return scsToInverseDynamicsJointMap.getInverseDynamicsSixDoFJoint(rootJoint); }
private FloatingJoint getRootJoint(SimulationConstructionSet scs0) { Joint firstRootJoint = scs0.getRobots()[0].getRootJoints().get(0); if (firstRootJoint instanceof FloatingJoint) return (FloatingJoint) firstRootJoint; throw new RuntimeException("first root joint is not a floating joint: " + firstRootJoint); }
private void PackSensorDataFromRobot() { for (Joint rootJoint : terminator.getRootJoints()) { recurseThrough(rootJoint); } }
public void getRobotInformationAsStringBuffer(StringBuffer buffer) { ArrayList<Joint> rootJoints = robot.getRootJoints(); for (Joint rootJoint : rootJoints) { buffer.append("Found Root Joint.\n"); printJointInformation(rootJoint, buffer); } }
private RigidBody getRootBody() { ArrayList<Joint> rootJoints = robot.getRootJoints(); if (rootJoints.size() > 1) throw new RuntimeException("Only works with one root joint"); Joint rootJoint = rootJoints.get(0); return scsToInverseDynamicsJointMap.getRigidBody(rootJoint); }
ConservedQuantitiesChecker(Robot robot) { this.robot = robot; this.rootJoint = (FloatingJoint) robot.getRootJoints().get(0); this.registry = new YoVariableRegistry(robot.getName() + getClass().getSimpleName()); }
public TorqueSpeedDataExporterGraphCreator(Robot robot, DataBuffer dataBuffer) { super(robot.getYoTime(), dataBuffer); for (Joint rootJoint : robot.getRootJoints()) { recursivelyAddPinJoints(rootJoint, pinJoints); } }
SinusoidalTorqueController(Robot robot) { this.t = robot.getYoTime(); this.registry = new YoVariableRegistry(robot.getName() + getClass().getSimpleName()); Joint rootJoint = robot.getRootJoints().get(0); for(Joint childJoint : rootJoint.getChildrenJoints()) { recursivelyAddJointTorqueProfile(childJoint); } }
private SDFModel createSDFModel() { SDFModel model = new SDFModel(); List<SDFJoint> sdfJoints = new ArrayList<>(); List<SDFLink> sdfLink = new ArrayList<>(); ArrayList<OneDegreeOfFreedomJoint> scsJoints = new ArrayList<>(); scsRobot.getAllOneDegreeOfFreedomJoints(scsJoints); if (scsRobot.getRootJoints().size() > 1) throw new RuntimeException("Cannot handle multiple root joints for now."); sdfLink.add(createSDFLink(scsRobot.getRootJoints().get(0).getLink(), true)); for (OneDegreeOfFreedomJoint scsJoint : scsJoints) { sdfJoints.add(createSDFJoint(scsJoint)); sdfLink.add(createSDFLink(scsJoint.getLink(), false)); } model.setName(sdfModelName); model.setJoints(sdfJoints); model.setLinks(sdfLink); return model; }
private double findAccelerationGreatestMagnitude() { FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0); double mag = scsFloatingJoint.getAngularAccelerationInBody().length(); Vector3D linearAcceleration = new Vector3D(); scsFloatingJoint.getLinearAccelerationInWorld(linearAcceleration); mag = Math.max(linearAcceleration.length(), mag); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) mag = Math.max(mag, Math.abs(joint.getQDD())); return mag; } }
private SDFModel createSDFModel() { SDFModel model = new SDFModel(); List<SDFJoint> sdfJoints = new ArrayList<>(); List<SDFLink> sdfLink = new ArrayList<>(); ArrayList<OneDegreeOfFreedomJoint> scsJoints = new ArrayList<>(); scsRobot.getAllOneDegreeOfFreedomJoints(scsJoints); if (scsRobot.getRootJoints().size() > 1) throw new RuntimeException("Cannot handle multiple root joints for now."); sdfLink.add(createSDFLink(scsRobot.getRootJoints().get(0).getLink(), true)); for (OneDegreeOfFreedomJoint scsJoint : scsJoints) { sdfJoints.add(createSDFJoint(scsJoint)); sdfLink.add(createSDFLink(scsJoint.getLink(), false)); } model.setName(sdfModelName); model.setJoints(sdfJoints); model.setLinks(sdfLink); return model; }
private List<SDFVisual> createSDFVisual(Link scsLink) { if (scsLink.getName().contains("body")) System.out.println(); List<SDFVisual> sdfVisuals = new ArrayList<>(); ArrayList<Graphics3DPrimitiveInstruction> graphics3dInstructions = scsLink.getLinkGraphics().getGraphics3DInstructions(); RigidBodyTransform parentJointTransformToWorld = new RigidBodyTransform(); Joint parentJoint = scsLink.getParentJoint(); if (scsRobot.getRootJoints().contains(parentJoint)) parentJoint.getTransformToWorld(parentJointTransformToWorld); SDFVisual sdfVisual = createSDFVisual(parentJointTransformToWorld, graphics3dInstructions); if (sdfVisual != null) sdfVisuals.add(sdfVisual); return sdfVisuals; }
private List<SDFVisual> createSDFVisual(Link scsLink) { if (scsLink.getName().contains("body")) System.out.println(); List<SDFVisual> sdfVisuals = new ArrayList<>(); ArrayList<Graphics3DPrimitiveInstruction> graphics3dInstructions = scsLink.getLinkGraphics().getGraphics3DInstructions(); RigidBodyTransform parentJointTransformToWorld = new RigidBodyTransform(); Joint parentJoint = scsLink.getParentJoint(); if (scsRobot.getRootJoints().contains(parentJoint)) parentJoint.getTransformToWorld(parentJointTransformToWorld); SDFVisual sdfVisual = createSDFVisual(parentJointTransformToWorld, graphics3dInstructions); if (sdfVisual != null) sdfVisuals.add(sdfVisual); return sdfVisuals; }
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 static void setRandomVelocity(Random random, Robot robot) { FloatingJoint rootJoint = (FloatingJoint) robot.getRootJoints().get(0); rootJoint.setVelocity(EuclidCoreRandomTools.nextVector3D(random)); rootJoint.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) joint.setQd(EuclidCoreRandomTools.nextDouble(random)); }
@Override public void waitAndWriteData(long tick) { if (tick % ticksPerSimulationTick == 0 && !(tick == 0 && skipFirstControlCycle)) { robotControlElement.write(System.nanoTime()); if (simulatedRobot != null) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); simulatedRobot.getRootJoints().get(0).getTransformToWorld(transformToWorld); if (robotControlElement.getYoGraphicsListRegistry() != null) { robotControlElement.getYoGraphicsListRegistry().setSimulationTransformToWorld(transformToWorld); } } } }
@Override public void updateYoGraphicsListRegistry() { if(robotControlElement.getYoGraphicsListRegistry() != null) { if(registry!=null) registry.updateChangedValues(); if (simulatedRobot != null) { simulatedRobot.getRootJoints().get(0).getTransformToWorld(transformToWorld); robotControlElement.getYoGraphicsListRegistry().setSimulationTransformToWorld(transformToWorld); } robotControlElement.getYoGraphicsListRegistry().update(); } }
public DataExporterExcelWorkbookCreator(Robot robot, DataBuffer dataBuffer) { this.robot = robot; for (Joint rootJoint : robot.getRootJoints()) { recursivelyAddPinJoints(rootJoint, pinJoints); recursivelyAddJoints(rootJoint, allJoints); } this.dataBuffer = dataBuffer; defaultFormat = new WritableCellFormat(); WritableFont headerFont = new WritableFont(WritableFont.ARIAL, 10, WritableFont.BOLD); headerCellFormat = new WritableCellFormat(headerFont); defaultNumberFormat = new WritableCellFormat(NumberFormats.FLOAT); smallNumberFormat = new WritableCellFormat(NumberFormats.EXPONENTIAL); }
private void getFloatingJointStateFromSCS() { FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0); RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D(); floatingJoint.getJointPose().set(jointTransform3D); floatingJoint.getFrameAfterJoint().update(); FrameVector3D linearVelocity = new FrameVector3D(); scsJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint()); floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity); }
private void checkRobotsHaveSameState(Robot robotA, Robot robotB) { Point3D centerOfMassA = new Point3D(); Point3D centerOfMassB = new Point3D(); double totalMassA = robotA.computeCenterOfMass(centerOfMassA); double totalMassB = robotB.computeCenterOfMass(centerOfMassB); Vector3D angularMomentumA = new Vector3D(); Vector3D angularMomentumB = new Vector3D(); robotA.computeAngularMomentum(angularMomentumA); robotB.computeAngularMomentum(angularMomentumB); Vector3D linearMomentumA = new Vector3D(); Vector3D linearMomentumB = new Vector3D(); robotA.computeLinearMomentum(linearMomentumA); robotB.computeLinearMomentum(linearMomentumB); assertEquals(totalMassA, totalMassB, 1e-7); EuclidCoreTestTools.assertTuple3DEquals(centerOfMassA, centerOfMassB, 1e-10); EuclidCoreTestTools.assertTuple3DEquals(linearMomentumA, linearMomentumB, 1e-10); EuclidCoreTestTools.assertTuple3DEquals(angularMomentumA, angularMomentumB, 1e-9); ArrayList<Joint> jointsA = robotA.getRootJoints(); ArrayList<Joint> jointsB = robotB.getRootJoints(); recursivelyTestJointStateIsTheSame(jointsA, jointsB); }