private static final void areJointsTheSame(JointBasics originalJoint, JointBasics targetJoint) { if(!(originalJoint.getClass().equals(targetJoint.getClass()) && originalJoint.getName().equals(targetJoint.getName()))) { throw new RuntimeException(originalJoint.getName() + " differs from " + targetJoint); } } }
public static JointBasics[] findJointsWithNames(JointBasics[] allJoints, String... jointNames) { Set<String> jointNameSet = new HashSet<>(Arrays.asList(jointNames)); JointBasics[] result = Stream.of(allJoints).distinct().filter(joint -> jointNameSet.contains(joint.getName())).toArray(JointBasics[]::new); if (result.length != jointNames.length) throw new RuntimeException("Not all joints could be found"); return result; }
public String getParentJointName() { return contactingRigidBody.getParentJoint().getName(); }
/** * Creates a descriptive {@code String} for this Jacobian containing information such as the * {@code jacobianFrame}, the list of the joint names, and the current value of the Jacobian * matrix. * * @return a descriptive {@code String} for this Jacobian. */ @Override public String toString() { StringBuilder builder = new StringBuilder(); builder.append("Jacobian. jacobianFrame = " + jacobianFrame + ". Joints:\n"); for (JointBasics joint : joints) { builder.append(joint.getClass().getSimpleName() + " " + joint.getName() + "\n"); } builder.append("\n"); builder.append(jacobian.toString()); return builder.toString(); }
private void addJoint(CollisionBoxProvider collissionBoxProvider, JointBasics joint) { List<CollisionShape> collisionMesh = collissionBoxProvider.getCollisionMesh(joint.getName()); if (collisionMesh != null) { trackingCollisionShapes.add(new TrackingCollisionShape(joint.getFrameAfterJoint(), collisionMesh)); } else { System.err.println(joint + " does not have a collission mesh"); } }
private GraphicsJoint createJoint(JointBasics inverseDynamicsJoint, Graphics3DNodeType nodeType, GraphicsObjectsHolder graphicsObjectsHolder, boolean useCollisionMeshes) { Graphics3DObject graphics3DObject; if (useCollisionMeshes) { graphics3DObject = generateGraphics3DObjectFromCollisionMeshes(graphicsObjectsHolder.getCollisionObjects(inverseDynamicsJoint.getName())); } else { graphics3DObject = graphicsObjectsHolder.getGraphicsObject(inverseDynamicsJoint.getName()); } CommonJoint wrapJointBasics = wrapJointBasics(inverseDynamicsJoint); GraphicsJoint graphicsJoint = new GraphicsJoint(inverseDynamicsJoint.getName(), wrapJointBasics, graphics3DObject, nodeType); registerJoint(wrapJointBasics, graphicsJoint); return graphicsJoint; }
/** * Performs a deep copy of {@code original}, preserving naming, root body, and the joints to ignore. * The clone is attached to the given {@code clonerootFrame}. * * @param original the multi-body system to clone. Not modified. * @param cloneRootFrame the root frame to which the clone system is attached. * @return the clone. */ public static MultiBodySystemBasics clone(MultiBodySystemReadOnly original, ReferenceFrame cloneRootFrame) { RigidBodyBasics cloneRootBody = MultiBodySystemFactories.cloneMultiBodySystem(original.getRootBody(), cloneRootFrame, ""); Set<String> namesOfJointsToConsider = SubtreeStreams.fromChildren(original.getRootBody()).map(JointReadOnly::getName).collect(Collectors.toSet()); List<? extends JointBasics> jointsToConsider = SubtreeStreams.fromChildren(cloneRootBody) .filter(joint -> namesOfJointsToConsider.contains(joint.getName())) .collect(Collectors.toList()); return toMultiBodySystemBasics(jointsToConsider); } }
DummyArmMomentumController(SCSRobotFromInverseDynamicsRobotModel scsRobot, FullRobotModel controllerModel, OneDoFJointBasics[] controlledJoints, List<VirtualModelControllerTestHelper.ForcePointController> forcePointControllers, VirtualModelMomentumController virtualModelController, List<RigidBodyBasics> endEffectors, List<YoFixedFrameWrench> yoDesiredWrenches, SelectionMatrix6D selectionMatrix) { this.scsRobot = scsRobot; this.controllerModel = controllerModel; this.controlledJoints = controlledJoints; this.forcePointControllers = forcePointControllers; this.virtualModelController = virtualModelController; this.endEffectors = endEffectors; this.selectionMatrix = selectionMatrix; this.yoDesiredWrenches = yoDesiredWrenches; for (JointBasics joint : controlledJoints) yoJointTorques.put(joint, new YoDouble(joint.getName() + "solutionTorque", registry)); for (VirtualModelControllerTestHelper.ForcePointController forcePointController : forcePointControllers) registry.addChild(forcePointController.getYoVariableRegistry()); }
DummyArmController(SCSRobotFromInverseDynamicsRobotModel scsRobot, FullRobotModel controllerModel, OneDoFJointBasics[] controlledJoints, List<ForcePointController> forcePointControllers, VirtualModelController virtualModelController, List<RigidBodyBasics> endEffectors, List<YoFixedFrameWrench> yoDesiredWrenches, DenseMatrix64F selectionMatrix) { this.scsRobot = scsRobot; this.controllerModel = controllerModel; this.controlledJoints = controlledJoints; this.forcePointControllers = forcePointControllers; this.virtualModelController = virtualModelController; this.endEffectors = endEffectors; this.selectionMatrix = selectionMatrix; this.yoDesiredWrenches = yoDesiredWrenches; for (JointBasics joint : controlledJoints) yoJointTorques.put(joint, new YoDouble(joint.getName() + "solutionTorque", registry)); for (ForcePointController forcePointController : forcePointControllers) registry.addChild(forcePointController.getYoVariableRegistry()); }
public PushRobotController(FloatingRootJointRobot pushableRobot, FullHumanoidRobotModel fullRobotModel) { this(pushableRobot, fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, 0.3), 0.005); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testCreateJointPath() { int numberOfJoints = joints.size(), numberOfBodies = numberOfJoints + 1; RigidBodyBasics[] allBodies = new RigidBodyBasics[numberOfBodies]; allBodies[0] = elevator; for(int i = 0; i < numberOfJoints; i++) { allBodies[i+1] = joints.get(i).getSuccessor(); } RigidBodyBasics start = allBodies[0] , end = allBodies[allBodies.length - 1]; JointBasics[] jointPath = MultiBodySystemTools.createJointPath(start, end); for(int i = 0; i < jointPath.length; i++) { assertTrue(jointPath[i].getName().equalsIgnoreCase("chainCjoint" + i)); } }
public ForceSensorDefinition(String sensorName, RigidBodyBasics rigidBody, RigidBodyTransform transformFromSensorToParentJoint) { this.sensorName = sensorName; this.rigidBody = rigidBody; JointBasics parentJoint = rigidBody.getParentJoint(); this.parentJointName = parentJoint.getName(); this.transformFromSensorToParentJoint = new RigidBodyTransform(transformFromSensorToParentJoint); ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); sensorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(sensorName + "Frame", frameAfterJoint, transformFromSensorToParentJoint); }
private SideDependentList<String> getFootJointNames(FullHumanoidRobotModel fullRobotModel) { SideDependentList<String> jointNames = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { jointNames.put(robotSide, fullRobotModel.getFoot(robotSide).getParentJoint().getName()); } return jointNames; }
private SideDependentList<String> getFootJointNames(FullHumanoidRobotModel fullRobotModel) { SideDependentList<String> jointNames = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { jointNames.put(robotSide, fullRobotModel.getFoot(robotSide).getParentJoint().getName()); } return jointNames; }
public ForwardDynamicComparisonScript(Robot robot, DRCRobotModel robotModel) { this.robot = robot; FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); List<JointBasics> ignoredJoints = robotModel.getJointMap().getLastSimulatedJoints().stream() .flatMap(name -> SubtreeStreams.fromChildren(fullRobotModel.getOneDoFJointByName(name).getSuccessor())) .collect(Collectors.toList()); multiBodySystem = MultiBodySystemBasics.toMultiBodySystemBasics(fullRobotModel.getElevator(), ignoredJoints); calculator = new ForwardDynamicsCalculator(multiBodySystem); floatingJoint = fullRobotModel.getRootJoint(); allOneDoFJoints = MultiBodySystemTools.filterJoints(multiBodySystem.getJointsToConsider(), OneDoFJointBasics.class); multiBodySystem.getAllJoints().forEach(joint -> nameToJointMap.put(joint.getName(), joint)); }
public JointAxisVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints()); while (!jointStack.isEmpty()) { JointBasics joint = jointStack.get(0); if(joint instanceof OneDoFJointBasics) { FrameVector3DReadOnly jAxis=((OneDoFJointBasics)joint).getJointAxis(); ReferenceFrame referenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(joint.getName()+"JointAxis", new FramePoint3D(jAxis.getReferenceFrame()), new FrameVector3D(jAxis.getReferenceFrame(),jAxis)); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame , registry, false, length, YoAppearance.Gold()); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); } List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
private static CenterOfMassCalculator createCenterOfMassCalculatorInJointZUpFrame(RigidBodyBasics rootBody, boolean preserveY) { if (DEBUG) System.out.println("\nCenterOfMassCalibrationTool: rootBody = " + rootBody); JointBasics parentJoint = rootBody.getParentJoint(); if (DEBUG) System.out.println("parentJoint = " + parentJoint); ReferenceFrame jointFrame = parentJoint.getFrameAfterJoint(); if (DEBUG) System.out.println("jointFrame = " + jointFrame); String jointName = parentJoint.getName(); if (DEBUG) System.out.println("jointName = " + jointName); ReferenceFrame jointZUpFrame; if (preserveY) { jointZUpFrame = new ZUpPreserveYReferenceFrame(ReferenceFrame.getWorldFrame(), jointFrame, jointName + "ZUp"); } else { jointZUpFrame = new ZUpFrame(ReferenceFrame.getWorldFrame(), jointFrame, jointName + "ZUp"); } CenterOfMassCalculator centerOfMassCalculator = new CenterOfMassCalculator(rootBody, jointZUpFrame); return centerOfMassCalculator; } }
private DRCRobotModel setup(DRCStartingLocation startingLocation) throws SimulationExceededMaximumTimeException { String className = getClass().getSimpleName(); FlatGroundEnvironment environment = new FlatGroundEnvironment(); DRCRobotModel robotModel = getRobotModel(); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, robotModel, environment); drcSimulationTestHelper.setStartingLocation(startingLocation); drcSimulationTestHelper.createSimulation(className); drcSimulationTestHelper.getSimulationConstructionSet().setCameraPosition(0.0, -3.0, 1.0); drcSimulationTestHelper.getSimulationConstructionSet().setCameraFix(0.0, 0.0, 0.2); pushController = new PushRobotController(drcSimulationTestHelper.getRobot(), robotModel.createFullRobotModel().getChest().getParentJoint().getName(), new Vector3D(0, 0, 0.15)); ThreadTools.sleep(1000); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.5)); return robotModel; }
@Before public void setup() { MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test."); FlatGroundEnvironment flatGround = new FlatGroundEnvironment(); String className = getClass().getSimpleName(); robotModel = getRobotModel(); fullRobotModel = robotModel.createFullRobotModel(); PrintTools.debug("simulationTestingParameters.getKeepSCSUp " + simulationTestingParameters.getKeepSCSUp()); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setTestEnvironment(flatGround); drcSimulationTestHelper.setStartingLocation(getStartingLocation()); drcSimulationTestHelper.createSimulation(className); double z = getForcePointOffsetZInChestFrame(); pushRobotController = new PushRobotController(drcSimulationTestHelper.getRobot(), fullRobotModel.getPelvis().getParentJoint().getName(), new Vector3D(0, 0, z)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); scs.addYoGraphic(pushRobotController.getForceVisualizer()); for (RobotSide robotSide : RobotSide.values) { String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); @SuppressWarnings("unchecked") final YoEnum<ConstraintType> footConstraintType = (YoEnum<ConstraintType>) scs .getVariable(sidePrefix + "FootControlModule", sidePrefix + "FootCurrentState"); @SuppressWarnings("unchecked") final YoEnum<WalkingStateEnum> walkingState = (YoEnum<WalkingStateEnum>) scs .getVariable(WalkingHighLevelHumanoidController.class.getSimpleName(), "walkingCurrentState"); singleSupportStartConditions.put(robotSide, new SingleSupportStartCondition(footConstraintType)); } }
protected void setupTest(DRCRobotModel robotModel) throws SimulationExceededMaximumTimeException, InterruptedException { boolean runMultiThreaded = false; setupTrack(runMultiThreaded, robotModel); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); HumanoidFloatingRootJointRobot robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot(); // pushRobotController = new PushRobotController(robot, fullRobotModel); pushRobotController = new PushRobotController(robot, fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, getPushPointFromChestZOffset())); if (VISUALIZE_FORCE) { drcFlatGroundWalkingTrack.getSimulationConstructionSet().addYoGraphic(pushRobotController.getForceVisualizer()); } SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet(); YoBoolean enable = (YoBoolean) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery"); // enable push recovery enable.set(true); for (RobotSide robotSide : RobotSide.values) { String prefix = fullRobotModel.getFoot(robotSide).getName(); scs.getVariable(prefix + "FootControlModule", prefix + "State"); @SuppressWarnings("unchecked") final YoEnum<WalkingStateEnum> walkingState = (YoEnum<WalkingStateEnum>) scs.getVariable("WalkingHighLevelHumanoidController", "walkingState"); doubleSupportStartConditions.put(robotSide, new DoubleSupportStartCondition(walkingState, robotSide)); } }