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); } } }
private static 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 " + DataExporterExcelWorkbookCreator.class.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)); } } }
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)); } } } }
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);
String jointName = joint.getName(); if (jointName.equals(sdfJointNameMap.getJointBeforeFootName(robotSide)))
private void printJointInformation(Joint joint, StringBuffer buffer) String jointName = joint.getName();
private void changeAppendageGroundContactPointsToNewOffsets(HumanoidFloatingRootJointRobot robot, ArrayList<? extends Point2DBasics> contactPointsInAnkleFrame, String jointName, RobotSide robotSide) { double time = robot.getTime(); System.out.println("Changing contact points at time " + time); int pointIndex = 0; ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint point : allGroundContactPoints) { Joint parentJoint = point.getParentJoint(); if (parentJoint.getName().equals(jointName)) { Point2DBasics newContactPoint = contactPointsInAnkleFrame.get(pointIndex); point.setIsInContact(false); Vector3D offset = new Vector3D(); point.getOffset(offset); // System.out.println("originalOffset = " + offset); offset.setX(newContactPoint.getX()); offset.setY(newContactPoint.getY()); // System.out.println("newOffset = " + offset); point.setOffsetJoint(offset); pointIndex++; } } if (footContactsInAnkleFrame != null) { footContactsInAnkleFrame.set(robotSide, contactPointsInAnkleFrame); } }
private void changeAppendageGroundContactPointsToNewOffsets(HumanoidFloatingRootJointRobot robot, ArrayList<Point2D> newContactPoints, String jointName, RobotSide robotSide) { double time = robot.getTime(); System.out.println("Changing contact points at time " + time); int pointIndex = 0; ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint point : allGroundContactPoints) { Joint parentJoint = point.getParentJoint(); if (parentJoint.getName().equals(jointName)) { Point2D newContactPoint = newContactPoints.get(pointIndex); point.setIsInContact(false); Vector3D offset = new Vector3D(); point.getOffset(offset); // System.out.println("originalOffset = " + offset); offset.setX(newContactPoint.getX()); offset.setY(newContactPoint.getY()); // System.out.println("newOffset = " + offset); point.setOffsetJoint(offset); pointIndex++; } } if (footContactsInAnkleFrame != null) { footContactsInAnkleFrame.set(robotSide, newContactPoints); } }
private void setNewContacts() { String footJointName = drcSimulationTestHelper.getControllerFullRobotModel().getFoot(robotSide).getParentJoint().getName(); HumanoidFloatingRootJointRobot robot = drcSimulationTestHelper.getRobot(); int pointIndex = 0; ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint point : allGroundContactPoints) { Joint parentJoint = point.getParentJoint(); if (parentJoint.getName().equals(footJointName)) { Point2D newContactPoint = newContactPoints.get(pointIndex); point.setIsInContact(false); Vector3D offset = new Vector3D(); point.getOffset(offset); offset.setX(newContactPoint.getX()); offset.setY(newContactPoint.getY()); point.setOffsetJoint(offset); pointIndex++; } } if (footContactsInAnkleFrame != null) { footContactsInAnkleFrame.set(robotSide, newContactPoints); } setNewContactPoints.set(false); }
if (currentSCSJoint.getName().equals(idJoint.getPredecessor().getParentJoint().getName()))
public SDFQuadrupedPerfectSimulatedSensor(FloatingRootJointRobot sdfRobot, FullQuadrupedRobotModel fullRobotModel, CommonQuadrupedReferenceFrames referenceFrames) { super(sdfRobot, fullRobotModel, referenceFrames); sensorOneDoFJoints = fullRobotModel.getOneDoFJoints(); //FootSwitches ArrayList<GroundContactPoint> groundContactPoints = sdfRobot.getAllGroundContactPoints(); for(RobotQuadrant quadrant : RobotQuadrant.values) { String prefix = quadrant.getCamelCaseNameForStartOfExpression(); OneDoFJoint jointBeforeFoot = fullRobotModel.getOneDoFJointBeforeFoot(quadrant); for(GroundContactPoint groundContactPoint : groundContactPoints) { if(groundContactPoint.getParentJoint().getName().equals(jointBeforeFoot.getName())) { footSwitches.set(quadrant, new SimulatedContactBasedFootSwitch(prefix + groundContactPoint.getName(), groundContactPoint, super.getYoVariableRegistry())); } } } enableDrives = new BooleanYoVariable("enableDrives", getYoVariableRegistry()); enableDrives.set(true); }
private void getExternalWrenchesFromSCS() { calculator.setExternalWrenchesToZero(); for (ExternalForcePoint efp : robot.getAllGroundContactPoints()) { String parentJointName = efp.getParentJoint().getName(); RigidBodyBasics body = nameToJointMap.get(parentJointName).getSuccessor(); FrameVector3DReadOnly moment = efp.getYoMoment(); FrameVector3DReadOnly force = efp.getYoForce(); FramePoint3D pointOfApplication = new FramePoint3D(efp.getYoPosition()); pointOfApplication.changeFrame(body.getBodyFixedFrame()); SpatialVector vector6D = new SpatialVector(moment, force); vector6D.changeFrame(body.getBodyFixedFrame()); Wrench externalWrench = new Wrench(body.getBodyFixedFrame(), body.getBodyFixedFrame()); externalWrench.set(vector6D.getAngularPart(), vector6D.getLinearPart(), pointOfApplication); calculator.getExternalWrench(body).add(externalWrench); } }
String jointName = joint.getName(); addStringToSheet(configDataSheet, column++, row, jointName);
OneDoFJointBasics oneDoFJoint; if (joint instanceof OneDegreeOfFreedomJoint) oneDoFJoint = fullRobotModel.getOneDoFJointByName(joint.getName()); else throw new RuntimeException("Force sensor is only supported for OneDegreeOfFreedomJoint.");