private void printJointInformation(Joint joint, StringBuffer buffer) String jointName = joint.getName(); joint.getOffset(offset); joint.getJointAxis(jointAxis); Link link = joint.getLink(); printLinkInformation(link, buffer); ArrayList<Joint> childrenJoints = joint.getChildrenJoints();
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; }
GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + joint.getName() + "_" + j, robot.getRobotsYoVariableRegistry()); joint.addGroundContactPoint(groundIdentifier, contactPoint); allGroundContactPoints.get(i).add(contactPoint); YoBoolean contactAvailable = new YoBoolean("contact_" + name + "_" + joint.getName() + "_" + j + "_avail", robot.getRobotsYoVariableRegistry()); contactAvailable.set(true); contactsAvailable.get(i).add(contactAvailable); YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + joint.getName() + j, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" +joint.getName() + j, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector);
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 void recursivelyAddPinJoints(Joint joint, List<PinJoint> pinJoints) { if (joint instanceof PinJoint) pinJoints.add((PinJoint) joint); else if (DEBUG && !(joint instanceof FloatingJoint)) PrintTools.error("Joint " + joint.getName() + " not currently handled by " + getClass().getSimpleName()); for (Joint child : joint.getChildrenJoints()) { recursivelyAddPinJoints(child, pinJoints); } } }
public static void getJointUpdaterList(ArrayList<Joint> rootJoints, List<JointState<? extends Joint>> jointStates, ArrayList<JointUpdater> jointUpdatersToPack) { HashMap<String, Joint> joints = new HashMap<String, Joint>(); for (Joint joint : rootJoints) { joints.put(joint.getName(), joint); ArrayList<Joint> childeren = new ArrayList<Joint>(); joint.recursiveGetChildrenJoints(childeren); for (Joint child : childeren) { joints.put(child.getName(), child); } } for (JointState<?> jointState : jointStates) { Joint joint = joints.get(jointState.getName()); if (joint == null) { System.err.println("Cannot find joint " + jointState.getName()); continue; } jointUpdatersToPack.add(new JointUpdater(joint, jointState)); } } }
String linkName = jointToAddExternalForcePoints.getLink().getName(); ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp); jointToAddExternalForcePoints.addExternalForcePoint(efp);
handControlFramePositionInWorld = new Point3D(); wristJoint.addExternalForcePoint(efpWrist); wristJoint.addExternalForcePoint(efpHandControlFrame); wristJoint.getTransformToWorld(transform); wristJointPose = new FramePose3D(HumanoidReferenceFrames.getWorldFrame(), transform); yoWristJointPose = new YoFramePoseUsingYawPitchRoll("wristJointPose", HumanoidReferenceFrames.getWorldFrame(), registry); YoGraphicCoordinateSystem yoWristCoordinateSystem = new YoGraphicCoordinateSystem("wristCoordinateSystemViz", yoWristJointPose, 0.1, YoAppearance.Red()); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose = new FramePose3D(HumanoidReferenceFrames.getWorldFrame(), transform);
public void packFootForceSensors(SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors) { ArrayList<WrenchCalculatorInterface> forceSensors = new ArrayList<WrenchCalculatorInterface>(); sdfRobot.getForceSensors(forceSensors); SideDependentList<String> jointNamesBeforeFeet = sdfRobot.getJointNamesBeforeFeet(); for (RobotSide robotSide : RobotSide.values) { footForceSensors.put(robotSide, new ArrayList<WrenchCalculatorInterface>()); for (int i = 0; i < forceSensors.size(); i++) { if (forceSensors.get(i).getJoint().getName().equals(jointNamesBeforeFeet.get(robotSide))) { footForceSensors.get(robotSide).add(forceSensors.get(i)); } } } }
private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
private static void recursivelyAddJoints(Joint joint, List<Joint> allJoints) { allJoints.add(joint); for (Joint child : joint.getChildrenJoints()) { recursivelyAddJoints(child, allJoints); } } }
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; }
leftAnkleJoint.addIMUMount(leftIMUMount); IMUMount rightIMUMount = new IMUMount("rightIMU", new RigidBodyTransform(), robot); rightAnkleJoint.addIMUMount(rightIMUMount); leftAnkleJoint.addExternalForcePoint(leftFootExternalForcePoint); rightAnkleJoint.addExternalForcePoint(rightFootExternalForcePoint);
public DRCDrillEnvironment() { double forceVectorScale = 1.0 / 500.0; combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); combinedTerrainObject.addTerrainObject(setUpGround("Ground", tableCenter, 0.1)); RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle4d(), tableCenter); drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform , drillRadius, drillHeight, drillMass, "models/drill.obj"); final int groundContactGroupIdentifier = 0; drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true); for (int i = 0; i < 4; i++) { double angle = i * 2.0 * Math.PI / 4.0; double x = 1.5 * drillRadius * Math.cos(angle); double y = 1.5 * drillRadius * Math.sin(angle); GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3d(x, y, 0.0), drillRobot); drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint); } GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier,1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(combinedTerrainObject); drillRobot.setGroundContactModel(groundContactModel); robots.add(drillRobot); }
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; }
String linkName = jointToAddExternalForcePoints.getLink().getName(); ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp); jointToAddExternalForcePoints.addExternalForcePoint(efp);
handControlFramePositionInWorld = new Point3d(); wristJoint.addExternalForcePoint(efpWrist); wristJoint.addExternalForcePoint(efpHandControlFrame); wristJoint.getTransformToWorld(transform); wristJointPose = new FramePose(HumanoidReferenceFrames.getWorldFrame(), transform); yoWristJointPose = new YoFramePose("wristJointPose", HumanoidReferenceFrames.getWorldFrame(), registry); YoGraphicCoordinateSystem yoWristCoordinateSystem = new YoGraphicCoordinateSystem("wristCoordinateSystemViz", yoWristJointPose, 0.1, YoAppearance.Red()); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose = new FramePose(HumanoidReferenceFrames.getWorldFrame(), transform);