/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFramePose3D duplicate(YoVariableRegistry newRegistry) { return new YoFramePose3D(position.duplicate(newRegistry), orientation.duplicate(newRegistry)); }
YoFramePoint3D position = new YoFramePoint3D(x, y, z, worldFrame); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, worldFrame); currentRegionPose = new YoFramePose3D(position, orientation);
int numberOfPolynomials = random.nextInt(20) + 3; YoFramePose3D poseToPolynomialFrame = new YoFramePose3D(name + "Pose", ReferenceFrame.getWorldFrame(), registry);
isPlanarRegionsListComplete = new YoBoolean(name + "IsComplete", registry); clear = new YoBoolean(name + "Clear", registry); currentRegionPose = new YoFramePose3D(name + "CurrentRegionPose", worldFrame, registry);
private static YoFramePose3D findYoFramePose3D(String nameSpace, String namePrefix, String nameSuffix, SimulationConstructionSet scs) { YoDouble x = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(namePrefix, nameSuffix)); YoDouble y = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(namePrefix, nameSuffix)); YoDouble z = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createZName(namePrefix, nameSuffix)); YoFramePoint3D position = new YoFramePoint3D(x, y, z, ReferenceFrame.getWorldFrame()); YoDouble qx = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(namePrefix, nameSuffix)); YoDouble qy = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(namePrefix, nameSuffix)); YoDouble qz = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(namePrefix, nameSuffix)); YoDouble qs = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(namePrefix, nameSuffix)); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); return new YoFramePose3D(position, orientation); }
public HumanoidKinematicsToolboxController(CommandInputManager commandInputManager, StatusMessageOutputManager statusOutputManager, FullHumanoidRobotModel desiredFullRobotModel, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { super(commandInputManager, statusOutputManager, desiredFullRobotModel.getRootJoint(), getAllJointsExcludingHands(desiredFullRobotModel), createListOfControllableRigidBodies(desiredFullRobotModel), yoGraphicsListRegistry, parentRegistry); this.desiredFullRobotModel = desiredFullRobotModel; footWeight.set(200.0); momentumWeight.set(1.0); for (RobotSide robotSide : RobotSide.values) { if (desiredFullRobotModel.getHand(robotSide) != null) setupVisualization(desiredFullRobotModel.getHand(robotSide)); setupVisualization(desiredFullRobotModel.getFoot(robotSide)); } for (RobotSide robotSide : RobotSide.values) { String side = robotSide.getCamelCaseNameForMiddleOfExpression(); String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); isFootInSupport.put(robotSide, new YoBoolean("is" + side + "FootInSupport", registry)); initialFootPoses.put(robotSide, new YoFramePose3D(sidePrefix + "FootInitial", worldFrame, registry)); } populateJointLimitReductionFactors(); }
YoDouble qs = (YoDouble) yoVariables[index++]; YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); poseToWorldFrame = new YoFramePose3D(position, orientation);
int numberOfSteps = 3; YoFramePose3D footstepSolution = new YoFramePose3D("footstepSolution", ReferenceFrame.getWorldFrame(), registry); YoFramePoint2D unclippedFootstepSolution = new YoFramePoint2D("unclippedFootstepSolution", ReferenceFrame.getWorldFrame(), registry); FramePose3D footstepPose = new FramePose3D();
double stanceWidth = 0.2; int numberOfSteps = 3; YoFramePose3D foostepSolution = new YoFramePose3D("footstepSolution", ReferenceFrame.getWorldFrame(), registry); YoFramePoint2D unclippedFootstepSolution = new YoFramePoint2D("unclippedFootstepSolution", ReferenceFrame.getWorldFrame(), registry); FramePose3D foostepPose = new FramePose3D();
double stanceWidth = 0.2; int numberOfSteps = 3; YoFramePose3D foostepSolution = new YoFramePose3D("footstepSolution", ReferenceFrame.getWorldFrame(), registry); YoFramePoint2D unclippedFootstepSolution = new YoFramePoint2D("unclippedFootstepSolution", ReferenceFrame.getWorldFrame(), registry); FramePose3D footstepPose = new FramePose3D();