public void getSoleFramePose(FramePose soleFramePoseToPack) { soleFramePoseToPack.setIncludingFrame(soleFramePose); }
public void getDesiredPose(FramePose desiredPoseToPack) { desiredPoseToPack.setIncludingFrame(desiredPose); }
public void setSoleFramePose(FramePose soleFramePose) { this.soleFramePose.setIncludingFrame(soleFramePose); }
public void setGoalPose(FramePose goalPose) { goalPose.setIncludingFrame(goalPose); }
public void getReportedGoalPoseWorldFrame(FramePose framePoseToPack) { framePoseToPack.setIncludingFrame(foundFiducialPose); }
public SimpleFootstep(RobotSide robotSide, FramePose soleFramePose) { this.robotSide = robotSide; this.soleFramePose.setIncludingFrame(soleFramePose); foothold.clear(); }
public void setInputs(FramePose goalPose, FramePose initialStanceFootPose, RobotSide initialStanceSide) { this.goalPose = goalPose; this.initialStanceSide = initialStanceSide; this.initialStanceFootPose.setIncludingFrame(initialStanceFootPose); this.initialStanceFootPose.changeFrame(ReferenceFrame.getWorldFrame()); }
@Override public void set(LidarScanCommand other) { timestamp = other.timestamp; lidarPose.setIncludingFrame(other.lidarPose); scan.clear(); for (int i = 0; i < other.scan.size(); i++) scan.add().set(other.scan.get(i)); }
swingStartPose.setIncludingFrame(stanceFootPose); stanceFootPose.setIncludingFrame(swingEndPose);
framePose.setIncludingFrame(referenceFrame, framePose.getGeometryObject());
public void variableChanged(YoVariable<?> v) { if (userDoFootPose.getBooleanValue()) { userDesiredFootPose.getFramePose(framePose); ReferenceFrame soleZUpFrame = ankleZUpFrames.get(userFootPoseSide.getEnumValue()); soleZUpFrame.update(); framePose.setIncludingFrame(soleZUpFrame, framePose.getGeometryObject()); framePose.changeFrame(ReferenceFrame.getWorldFrame()); System.out.println("framePose " + framePose); FootTrajectoryCommand footTrajectoryControllerCommand = new FootTrajectoryCommand(); FrameSE3TrajectoryPoint trajectoryPoint = new FrameSE3TrajectoryPoint(ReferenceFrame.getWorldFrame()); trajectoryPoint.setTime(userDesiredFootPoseTrajectoryTime.getDoubleValue()); trajectoryPoint.setPosition(framePose.getFramePointCopy()); trajectoryPoint.setOrientation(framePose.getFrameOrientationCopy()); trajectoryPoint.setLinearVelocity(new Vector3d()); trajectoryPoint.setAngularVelocity(new Vector3d()); footTrajectoryControllerCommand.addTrajectoryPoint(trajectoryPoint); footTrajectoryControllerCommand.setRobotSide(userFootPoseSide.getEnumValue()); System.out.println("Submitting " + footTrajectoryControllerCommand); controllerCommandInputManager.submitCommand(footTrajectoryControllerCommand); userDoFootPose.set(false); } } });
swingStartPose.setIncludingFrame(stanceFootPose); stanceFootPose.setIncludingFrame(swingEndPose);
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(errorAxisAngle.getAngle()); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
swingStartPose.setIncludingFrame(stanceFootPose); stanceFootPose.setIncludingFrame(swingEndPose);
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle())); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }