private static HashSet<Link> getAllLinks(ArrayList<Joint> joints, HashSet<Link> links) { for (Joint joint : joints) { links.add(joint.getLink()); if (!joint.getChildrenJoints().isEmpty()) { links.addAll(getAllLinks(joint.getChildrenJoints(), links)); } } return links; }
private HashSet<Link> getAllLinks(ArrayList<Joint> joints, HashSet<Link> links) { for (Joint j : joints) { links.add(j.getLink()); if (!j.getChildrenJoints().isEmpty()) { links.addAll(getAllLinks(j.getChildrenJoints(), links)); } } return links; }
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; }
Link link = joint.getLink(); printLinkInformation(link, buffer);
String linkName = jointToAddExternalForcePoints.getLink().getName(); ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry());
String linkName = jointToAddExternalForcePoints.getLink().getName(); ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry());
String linkName = jointToAddExternalForcePoints.getLink().getName(); ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry());
private SDFJoint createSDFJoint(OneDegreeOfFreedomJoint scsJoint) { SDFJoint sdfJoint = new SDFJoint(); sdfJoint.setAxis(createSDFJointAxis(scsJoint)); sdfJoint.setChild(scsJoint.getLink().getName()); sdfJoint.setName(scsJoint.getName()); sdfJoint.setParent(scsJoint.getParentJoint().getLink().getName()); RigidBodyTransform scsJointOffset = new RigidBodyTransform(); scsJoint.getTransformToWorld(scsJointOffset); sdfJoint.setPose(getPoseFromTransform3D(scsJointOffset)); String type; if (scsJoint instanceof PinJoint) { type = "revolute"; } else { throw new RuntimeException("Implement me!"); } sdfJoint.setType(type); return sdfJoint; }
private SDFJoint createSDFJoint(OneDegreeOfFreedomJoint scsJoint) { SDFJoint sdfJoint = new SDFJoint(); sdfJoint.setAxis(createSDFJointAxis(scsJoint)); sdfJoint.setChild(scsJoint.getLink().getName()); sdfJoint.setName(scsJoint.getName()); sdfJoint.setParent(scsJoint.getParentJoint().getLink().getName()); RigidBodyTransform scsJointOffset = new RigidBodyTransform(); scsJoint.getTransformToWorld(scsJointOffset); sdfJoint.setPose(getPoseFromTransform3D(scsJointOffset)); String type; if (scsJoint instanceof PinJoint) { type = "revolute"; } else { throw new RuntimeException("Implement me!"); } sdfJoint.setType(type); return sdfJoint; }
Link link = polledJoint.getLink();
for (Joint joint : allJoints) Link link = joint.getLink(); int column = 0;
Link link = polledJoint.getLink();