public void getDesiredEndEffectorPoseFromQDesireds(FramePose desiredPose, ReferenceFrame desiredFrame) { desiredPose.setToZero(localControlFrame); desiredPose.changeFrame(desiredFrame); }
public void getFramePoseIncludingFrame(FramePose framePoseToPack) { framePoseToPack.setToZero(getReferenceFrame()); getFramePose(framePoseToPack); }
public void getPoseIncludingFrame(FramePose poseToPack) { poseToPack.setToZero(getReferenceFrame()); getPose(poseToPack); }
public void getSolePose(FramePose soleFrameToPack) { soleFrameToPack.setToZero(soleReferenceFrame); soleFrameToPack.changeFrame(ReferenceFrame.getWorldFrame()); }
public void getFramePoseIncludingFrame(FramePose framePoseToPack) { framePoseToPack.setToZero(getReferenceFrame()); getFramePose(framePoseToPack); }
private void setControlFrameFixedInEndEffector(ReferenceFrame controlFrame) { controlFramePose.setToZero(controlFrame); controlFramePose.changeFrame(endEffectorFrame); spatialFeedbackControlCommand.setControlFrameFixedInEndEffector(controlFramePose); this.controlFrame.setPoseAndUpdate(controlFramePose); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { midFeetPose.setToZero(midFeetZUpWalkDirectionFrame); pelvisPose.setToZero(getPelvisFrame()); pelvisPose.changeFrame(midFeetZUpWalkDirectionFrame); midFeetPose.setX(pelvisPose.getX()); midFeetPose.changeFrame(ReferenceFrame.getWorldFrame()); midFeetPose.getPose(transformToParent); } };
public void variableChanged(YoVariable<?> v) { if (userUpdateDesiredPelvisPose.getBooleanValue()) { framePose.setToZero(pelvisFrame); framePose.changeFrame(midFeetZUpFrame); userDesiredPelvisPose.set(framePose); userUpdateDesiredPelvisPose.set(false); } } });
private void updateTangentialCircleFrame() { if (controlledFrame != null) { tangentialSteeringFramePose.setToZero(controlledFrame); } else { tangentialSteeringFramePose.setToZero(currentPosition.getReferenceFrame()); tangentialSteeringFramePose.setPosition(currentPosition); } tangentialSteeringFramePose.changeFrame(steeringWheelFrame); double x = tangentialSteeringFramePose.getX(); double y = tangentialSteeringFramePose.getY(); double yaw = trimAngleMinusPiToPi(Math.PI / 2.0 + Math.atan2(y, x)); tangentialSteeringFramePose.setYawPitchRoll(yaw, 0.0, 0.0); tangentialSteeringFrame.setPoseAndUpdate(tangentialSteeringFramePose); yoTangentialSteeringFramePose.setAndMatchFrame(tangentialSteeringFramePose); }
public Footstep getFootstepAtCurrentLocation(RobotSide robotSide) { tempPose.setToZero(contactableFeet.get(robotSide).getFrameAfterParentJoint()); tempPose.changeFrame(worldFrame); Footstep footstep = footstepsAtCurrentLocation.get(robotSide); footstep.setPose(tempPose); return footstep; }
private void updateTangentialCircleFrame() { if (controlledFrame != null) { tangentialCircleFramePose.setToZero(controlledFrame); } else { tangentialCircleFramePose.setToZero(currentPosition.getReferenceFrame()); tangentialCircleFramePose.setPosition(currentPosition); } tangentialCircleFramePose.changeFrame(circleFrame); double x = tangentialCircleFramePose.getX(); double y = tangentialCircleFramePose.getY(); double yaw = trimAngleMinusPiToPi(Math.PI / 2.0 + Math.atan2(y, x)); tangentialCircleFramePose.setYawPitchRoll(yaw, 0.0, 0.0); tangentialCircleFrame.setPoseAndUpdate(tangentialCircleFramePose); yoTangentialCircleFramePose.setAndMatchFrame(tangentialCircleFramePose); }
@Override public void doTransitionOutOfAction() { super.doTransitionOutOfAction(); balanceManager.minimizeAngularMomentumRateZ(false); actualFootPoseInWorld.setToZero(fullRobotModel.getEndEffectorFrame(swingSide, LimbName.LEG)); // changed Here Nicolas actualFootPoseInWorld.changeFrame(worldFrame); walkingMessageHandler.reportFootstepCompleted(swingSide, actualFootPoseInWorld); walkingMessageHandler.registerCompletedDesiredFootstep(nextFootstep); }
public void updateSteeringWheel(FramePoint center, FrameVector rotationAxis, FrameVector zeroAxis) { steeringWheelCenter.setAndMatchFrame(center); steeringWheelRotationAxis.setAndMatchFrame(rotationAxis); steeringWheelRotationAxis.normalize(); steeringWheelZeroAxis.setAndMatchFrame(zeroAxis); steeringWheelZeroAxis.normalize(); steeringWheelFrame.update(); steeringWheelFramePose.setToZero(steeringWheelFrame); steeringWheelFramePose.changeFrame(worldFrame); yoSteeringWheelFramePose.set(steeringWheelFramePose); }
public void setInitialLeadOut(FramePoint initialPosition, FrameVector initialDirection, double leaveDistance) { this.initialPosition.set(initialPosition); this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); initialDistortionPose.setToZero(this.initialPosition.getReferenceFrame()); initialDistortionPose.setPosition(initialPosition); initialDistortionPose.setOrientation(tempAxisAngle); this.leaveDistance.set(leaveDistance); }
public void setFinalLeadIn(FramePoint finalPosition, FrameVector finalDirection, double approachDistance) { this.finalPosition.set(finalPosition); this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); finalDistortionPose.setToZero(this.finalPosition.getReferenceFrame()); finalDistortionPose.setPosition(finalPosition); finalDistortionPose.setOrientation(tempAxisAngle); this.approachDistance.set(approachDistance); }
private void updateLocalBaseFrame() { originalBasePose.setToZero(originalBaseParentJointFrame); originalBasePose.changeFrame(worldFrame); originalBasePose.getPoseIncludingFrame(baseParentJointFramePosition, baseParentJointFrameOrientation); yoBaseParentJointFrameOrientation.set(baseParentJointFrameOrientation); yoBaseParentJointFramePosition.set(baseParentJointFramePosition); yoBaseParentJointFrameOrientationFiltered.update(); yoBaseParentJointFramePositionFiltered.update(); yoBaseParentJointFrameOrientationFiltered.getFrameOrientationIncludingFrame(baseParentJointFrameOrientation); yoBaseParentJointFramePositionFiltered.getFrameTupleIncludingFrame(baseParentJointFramePosition); localBaseParentJointFrame.setPoseAndUpdate(baseParentJointFramePosition, baseParentJointFrameOrientation); updateFrames(); }
@Override public void onBehaviorEntered() { hasTargetBeenProvided.set(false); haveFootstepsBeenGenerated.set(false); hasInputBeenSet.set(false); footstepListBehavior.initialize(); robotPose.setToZero(fullRobotModel.getRootJoint().getFrameAfterJoint()); robotPose.changeFrame(worldFrame); robotYoPose.set(robotPose); robotPose.getPosition(robotLocation); robotPose.getOrientation(robotOrientation); this.targetLocation.set(robotLocation); this.targetOrientation.set(robotOrientation); }
private void sequenceSquareUp() { FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime.getDoubleValue(), transferTime.getDoubleValue()); FramePose footstepPose = new FramePose(); RobotSide robotSide = activeSideForFootControl.getEnumValue(); if (robotSide == null) System.out.println("choose a foot to be squared up"); else { footstepPose.setToZero(fullRobotModel.getEndEffectorFrame(robotSide.getOppositeSide(), LimbName.LEG)); footstepPose.setY(robotSide.getOppositeSide().negateIfLeftSide(walkingControllerParameters.getInPlaceWidth())); footstepPose.changeFrame(worldFrame); Point3d footLocation = new Point3d(); Quat4d footOrientation = new Quat4d(); footstepPose.getPosition(footLocation); footstepPose.getOrientation(footOrientation); FootstepDataMessage footstepData = new FootstepDataMessage(robotSide, footLocation, footOrientation); footstepDataList.add(footstepData); pipeLine.submitSingleTaskStage(new FootstepListTask(footstepListBehavior, footstepDataList)); } }
private void kraneKick(RobotSide robotSide) { ReferenceFrame ankleZUpFrame = ankleZUpFrames.get(robotSide.getOppositeSide()); // First Lift up the foot submitFootPosition(false, robotSide, new FramePoint(ankleZUpFrame, 0.1, robotSide.negateIfRightSide(0.25), 0.2)); submitSymmetricHumanoidArmPose(HumanoidArmPose.KARATE_KID_KRANE_KICK); pipeLine.requestNewStage(); // Go back to stand prep but don't put the foot on the ground yet submitSymmetricHumanoidArmPose(HumanoidArmPose.STAND_PREP); FramePose footPose = new FramePose(); footPose.setToZero(ankleZUpFrame); footPose.setPosition(0.0, robotSide.negateIfRightSide(0.25), 0.1); footPose.setYawPitchRoll(0.0, 0.0, 0.0); submitFootPose(true, robotSide, footPose); submitChestHomeCommand(true); // Put the foot back on the ground submitFootPosition(false, robotSide, new FramePoint(ankleZUpFrame, 0.0, robotSide.negateIfRightSide(0.25), -0.3)); }
public void setGoalPose(FramePose goalPose) { goalPose.getPosition2dIncludingFrame(this.goalPosition); Tuple3d goalPosition = new Point3d(); goalPose.getPosition(goalPosition); FootstepPlannerGoal footstepPlannerGoal = new FootstepPlannerGoal(); footstepPlannerGoal.setGoalPoseBetweenFeet(goalPose); Point2d xyGoal = new Point2d(); xyGoal.setX(goalPose.getX()); xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal); footstepPlannerGoal.setFootstepPlannerGoalType(FootstepPlannerGoalType.CLOSE_TO_XY_POSITION); FramePose leftFootPose = new FramePose(); leftFootPose.setToZero(referenceFrames.getSoleFrame(RobotSide.LEFT)); leftFootPose.changeFrame(ReferenceFrame.getWorldFrame()); sendPacketToUI(new UIPositionCheckerPacket(new Point3d(xyGoal.getX(), xyGoal.getY(), leftFootPose.getZ()), new Quat4d())); footstepPlanner.setGoal(footstepPlannerGoal); footstepPlanner.setInitialStanceFoot(leftFootPose, RobotSide.LEFT); }