private static ArrayList<OneDegreeOfFreedomJoint> getAllRobotJoints(Robot robot) { ArrayList<OneDegreeOfFreedomJoint> ret = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(ret); return ret; }
public LinkComIDActionListener(DataBuffer dataBuffer, Robot robot) { this.dataBuffer = dataBuffer; this.robot = robot; ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); this.linkNames = new String[joints.size()]; for (int i = 0; i < linkNames.length; i++) { linkNames[i] = joints.get(i).getLink().getName(); } }
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)); }
Map<String, OneDegreeOfFreedomJoint> scsJointMap = new HashMap<>(); ArrayList<OneDegreeOfFreedomJoint> scsOnDoFJointList = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(scsOnDoFJointList); scsOnDoFJointList.forEach(joint -> scsJointMap.put(joint.getName(), joint));
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; } }
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); } }
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 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; }
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)); }
scsRobot.getAllOneDegreeOfFreedomJoints(allSCSRobotOneDoFJoints);