private void hideGraphics(YoGraphicPosition graphics) { graphics.setPositionToNaN(); graphics.update(); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).set(externalForcePoints.get(i).getYoPosition()); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { externalForcePoints.get(i).getYoPosition().get(initialPositions.get(i)); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
private void setGraphics(YoGraphicPosition graphics, FootstepNode node) { graphics.setPosition(node.getX(), node.getY(), 0.0); graphics.update(); }
private void updateContactPointDynamicGraphicObjects(int i, ContactPointInterface contactPoint) { if (contactPoint.isInContact()) { contactPoint.getPosition(tempFramePoint); tempFramePoint.changeFrame(worldFrame); contactPointsWorld.get(i).set(tempFramePoint); normalVectors.get(i).set(tempFrameVector); dynamicGraphicPositions.get(i).showGraphicObject(); dynamicGraphicVectors.get(i).showGraphicObject(); } else { contactPointsWorld.get(i).setToNaN(); normalVectors.get(i).set(Double.NaN, Double.NaN, Double.NaN); dynamicGraphicPositions.get(i).hideGraphicObject(); dynamicGraphicVectors.get(i).hideGraphicObject(); } dynamicGraphicPositions.get(i).update(); dynamicGraphicVectors.get(i).update(); } }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); efp.getYoPosition().get(proportionalTerm); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); efp.getYoVelocity().get(derivativeTerm); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
private boolean isPositionReachable(int xIndex, int yIndex, int zIndex) { voxel3dGrid.getVoxel(voxelLocation, xIndex, yIndex, zIndex); modifiableVoxelLocation.setIncludingFrame(voxelLocation); modifiableVoxelLocation.changeFrame(ReferenceFrame.getWorldFrame()); currentEvaluationPosition.setPosition(modifiableVoxelLocation); currentEvaluationPosition.update(); return solver.solveFor(voxelLocation); }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); proportionalTerm.set(efp.getYoPosition()); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); derivativeTerm.set(efp.getYoVelocity()); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
footPositionGraphics.get(robotQuadrant).update();